#include "roboeasy.h" #include "robomaster.h" #include "connection.h" #include #include #include 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; }