robomaster-sdk/src/robo.c

#include "roboeasy.h"
#include "robomaster.h"
#include "connection.h"

#include <string.h>
#include <stdlib.h>
#include <math.h>

#include <stdio.h>

struct FragmentLink {
    struct Fragment fragment;
    struct FragmentLink *next;
};

struct RobotImp {

    struct {
        struct FragmentLink *head;
        struct FragmentLink *free;
    } frag_queue;

    struct ClientImp* client;
    uint16_t seq;

    int16_t wheels[4];
    bool dirty_wheels;

    uint8_t colors[3];
    bool dirty_colors;

    int16_t gimbal[2];
    bool dirty_gimbal;

    int16_t pitch;
    int16_t yaw;
    bool dirty_target;

    bool dirty_blaster;

    bool stream_state;
    bool dirty_stream;

    bool sdk_mode;

    enum {
        CONNECTING_TO_PROXY_PORT,
        SETTING_SDK_CONNECTION,
        CONNECTING_TO_DEVICE_PORT,
        SETTING_SDK_MODE,
        RESETTING_SUBNODE,
        SUSBCRIBING_SUBNODE,
        SETTING_MOVEMENT_MODE,
        SENDING_HEARTBEAT,
        WAITING,
        READY,
        STOPPED
    } state;

};

int
robot_stream(Robot robot, bool state) {
    robot->stream_state = state;
    robot->dirty_stream = true;
    return 1;
}

int
robot_blast(Robot robot) {
    robot->dirty_blaster = true;
    return 1;
}

int
robot_drive(Robot robot, float x, float y, float r) {
    // TODO: move individual wheel speed calculation to work function
    // This function may be used in a timer handler, so fast as possible
    robot->wheels[0] = (y + x + r) * 1000;
    robot->wheels[1] = -(y - x - r) * 1000;
    robot->wheels[2] = -(y + x - r) * 1000;
    robot->wheels[3] = (y - x + r) * 1000;
    robot->dirty_wheels = true;
    return 1;
}

int
robot_aim(Robot robot, float p, float y) {
    robot->gimbal[0] = p * 360;
    robot->gimbal[1] = y * 360;
    robot->dirty_gimbal = true;
    return 1;
}

int
robot_target(Robot robot, float pitch, float yaw) {
    // Get the FOV angle of the point in radians
    float FOV = 120 * (M_PI / 180);
    yaw = yaw * (FOV / 2);
    pitch = pitch * (FOV / 2);

    // Convert to degrees
    yaw = yaw * (180 / M_PI);
    pitch = pitch * (180 / M_PI);

    // Convert for Robomaster
    robot->yaw = yaw * 10;
    robot->pitch = -pitch * 10;

    robot->dirty_target = true;
    return 1;
};

int
robot_led(Robot robot, unsigned char r, unsigned char g, unsigned char b) {
    robot->colors[0] = r;
    robot->colors[1] = g;
    robot->colors[2] = b;
    robot->dirty_colors = true;
    return 1;
}

int
robot_heartbeat(Robot robot) {

    if(robot->state != READY)
        return 0;

    robot->state = SENDING_HEARTBEAT;

    return 1;

}

inline
static
void
enqueue_fragment (
        Robot robot,
        const struct Fragment fragment ) {

    struct FragmentLink *link;
    if(robot->frag_queue.free) {
        link = robot->frag_queue.free;
        robot->frag_queue.free = link->next;
    } else {
        link = malloc(sizeof(struct FragmentLink));
        link->next = NULL;
    }

    link->fragment = fragment;

    if(robot->frag_queue.head) {
        robot->frag_queue.head->next = link;
    } else {
        robot->frag_queue.head = link;
    }

}

inline
static
void
dequeue_fragment (
        Robot robot,
        struct Fragment *fragment ) {

    fragment->type = 0;
    if(!robot->frag_queue.head)
        return;

    struct FragmentLink *link = robot->frag_queue.head;
    *fragment = link->fragment;

    robot->frag_queue.head = link->next;
    link->next = NULL;
    if(robot->frag_queue.free) {
        robot->frag_queue.free->next = link;
    } else {
        robot->frag_queue.free = link;
    }

}

int robot_poll(Robot robot, struct Fragment* fragment) {
    dequeue_fragment(robot, fragment);
    return fragment->type;
}

int
robot_work(Robot robot) {

    union Request req = {0};

    switch(robot->state) {

        case WAITING:
            break;

        case CONNECTING_TO_PROXY_PORT:
        {
            robot->client->sdk_conn = connection_new(0, 0, 30030, "192.168.2.1");
            robot->state = SETTING_SDK_CONNECTION;
            break;
        }

        case SETTING_SDK_CONNECTION:
        {
            set_sdk_connection(&req, robot->seq++, true, CONNECTION_WIFI_AP, 0, 10010);
            req_send(robot->client->sdk_conn, &req);
            robot->state = WAITING;
            break;
        }

        case CONNECTING_TO_DEVICE_PORT:
        {
            robot->client->dev_conn = connection_new(10010, "192.168.2.24", 20020, "192.168.2.1");
            robot->state = SETTING_SDK_MODE;
            break;
        }

        case SETTING_SDK_MODE:
        {
            set_sdk_mode(&req, robot->seq++, true, robot->sdk_mode);
            req_send(robot->client->dev_conn, &req);
            robot->state = WAITING;
            break;
        }

        case RESETTING_SUBNODE:
        {
            subnode_reset(&req, robot->seq++, true);
            req_send(robot->client->dev_conn, &req);
            robot->state = WAITING;
            break;
        }

        case SUSBCRIBING_SUBNODE:
        {
            subscribe_add_node(&req, robot->seq++, true);
            req_send(robot->client->dev_conn, &req);
            robot->state = WAITING;
            break;
        }

        case SETTING_MOVEMENT_MODE:
        {
            set_robot_mode(&req, robot->seq++, true, MOVEMENTMODE_GIMBAL_LEAD);
            req_send(robot->client->dev_conn, &req);
            robot->state = WAITING;
            break;
        }

        case SENDING_HEARTBEAT:
        {
            sdk_heartbeat(&req, robot->seq++, true);
            req_send(robot->client->sdk_conn, &req);
            robot->state = WAITING;
            break;
        }

        case READY:
        {
            if(robot->dirty_wheels) {
                set_wheel_speed (
                        &req, robot->seq++, false,
                        robot->wheels[0],
                        robot->wheels[1],
                        robot->wheels[2],
                        robot->wheels[3] );
                req_send(robot->client->dev_conn, &req);
                robot->dirty_wheels = false;
            }
            if(robot->dirty_gimbal) {
                gimbal_ctrl_speed (
                        &req, robot->seq++, false,
                        robot->gimbal[0],
                        robot->gimbal[1],
                        0 );
                req_send(robot->client->dev_conn, &req);
                robot->dirty_gimbal = false;
            }
            if(robot->dirty_target) {
                gimbal_rotate (
                        &req, robot->seq++, true,
                        robot->pitch,
                        robot->yaw );
                req_send(robot->client->dev_conn, &req);
                robot->dirty_target = false;
            }
            if(robot->dirty_colors) {
                set_system_led (
                        &req, robot->seq++, false,
                        robot->colors[0],
                        robot->colors[1],
                        robot->colors[2],
                        LEDCOMP_ALL,
                        0xFFFF,
                        LEDEFFECT_ON,
                        100,
                        100 );
                req_send(robot->client->dev_conn, &req);
                robot->dirty_colors = false;
            }
            if(robot->dirty_blaster) {
                blaster_fire (
                    &req, robot->seq++, false,
                    FIRETYPE_INFRARED,
                    ONE
                );
                req_send(robot->client->dev_conn, &req);
                robot->dirty_blaster = false;
            }
            if(robot->dirty_stream) {
                stream_ctrl (
                    &req, robot->seq++, true,
                    robot->stream_state ? STREAMSTATE_ON : STREAMSTATE_OFF,
                    RES_720P,
                    STREAMCTRL_SDK
                );
                req_send(robot->client->dev_conn, &req);
                stream_ctrl (
                    &req, robot->seq++, true,
                    robot->stream_state ? STREAMSTATE_ON : STREAMSTATE_OFF,
                    RES_720P,
                    STREAMCTRL_VIDEO
                );
                req_send(robot->client->dev_conn, &req);
                robot->dirty_stream = false;
            }
            break;
        }

        case STOPPED:
            return 0;

    }

    union Message resp;
    poll_message(robot->client, &resp);
    if(resp.header.cmd) {
        const struct Fragment fragment = {
            .type = FRAGMENT_MESSAGE,
            .message = resp
        };
        enqueue_fragment(robot, fragment);
    }
    switch(resp.header.cmd) {
        case 0:
            break;
        case VISION_DETECT_INFO_CMD:
            // enqueue rectangles
            for(int i = 0; i < resp.push.vision.count; i++) {
                int type;
                switch(resp.push.vision.type) {
                    case VISIONTYPE_SHOULDER:
                    case VISIONTYPE_PERSON:
                    case VISIONTYPE_GESTURE:
                    case VISIONTYPE_ROBOT:
                        type = FRAGMENT_RECTANGLE;
                    case VISIONTYPE_LINE:
                        type = FRAGMENT_LINE;
                    case VISIONTYPE_MARKER:
                        type = FRAGMENT_MARKER;
                }
                const struct Fragment fragment = {
                    .type = type,
                    .rect = resp.push.vision.rects[i]
                };
                enqueue_fragment(robot, fragment);
            }
            break;
        case SET_SDK_CONNECTION_CMD:
            // TODO: Do more with this
            if(resp.resp.sdkconn.retcode) {
                robot->state = STOPPED;
                break;
            }
            robot->state = CONNECTING_TO_DEVICE_PORT;
            break;
        case SET_SDK_MODE_CMD:
            // TODO: Do more with this
            if(resp.resp.sdkmode.retcode) {
                robot->state = STOPPED;
                break;
            }
            robot->state = robot->sdk_mode ? RESETTING_SUBNODE : STOPPED;
            break;
        case SUBNODE_RESET_CMD:
            if(resp.resp.subnodereset.retcode) {
                robot->state = STOPPED;
                break;
            }
            robot->state = SUSBCRIBING_SUBNODE;
            break;
        case SUBSCRIBE_ADD_NODE_CMD:
            if(resp.resp.subnodeadd.retcode && resp.resp.subnodeadd.retcode != 0x50) {
                robot->state = STOPPED;
                break;
            }
            robot->state = SETTING_MOVEMENT_MODE;
            break;
        case SET_ROBOT_MODE_CMD:
            if(resp.resp.mvmode.retcode) {
                robot->state = STOPPED;
                break;
            }
            robot->state = SENDING_HEARTBEAT;
            break;
        case SDK_HEARTBEAT_CMD:
            if(resp.resp.heartbeat.retcode) {
                robot->state = STOPPED;
                break;
            }
            robot->state = READY;
            break;
    }

    return 1;

}

Robot robot_new() {
    struct RobotImp* robot = malloc(sizeof(struct RobotImp));
    memset(robot, 0, sizeof(struct RobotImp));
    return robot;
}

int robot_init(Robot robot) {
    robot->client = client_new();
    robot->sdk_mode = true;
    robot->state = CONNECTING_TO_PROXY_PORT;
    return 0;
}

int robot_stop(Robot robot) {
    robot->sdk_mode = false;
    robot->state = SETTING_SDK_MODE;
    return 0;
}