robomaster-sdk/src/robo.c

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

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

#include <stdio.h>

struct RobotImp {

    struct Client* 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;

    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_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;

}

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_FREE);
            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_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);
    switch(resp.header.cmd) {
        case 0:
            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;
}