diff --git a/CMakeLists.txt b/CMakeLists.txt index d248b78..9667c32 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -17,6 +17,7 @@ add_library(robomaster src/modules/chassis.c src/connection.c src/robomaster.c + src/robo.c ) target_include_directories(robomaster diff --git a/include/chassis.h b/include/chassis.h index cd7687d..5f364d3 100644 --- a/include/chassis.h +++ b/include/chassis.h @@ -8,21 +8,25 @@ static const uint8_t CHASSIS_HOST = 3; static const uint8_t CHASSIS_INDEX = 6; -static const uint16_t SET_WHEEL_SPEED_CMD = 0x203F; +#define SET_WHEEL_SPEED_CMD 0x203F void set_wheel_speed ( - Client session, + union Request* req, + uint16_t seq, + bool ack, int16_t w1, int16_t w2, int16_t w3, int16_t w4 ); -static const uint16_t CHASSIS_SPEED_MODE_CMD = 0x213F; +#define CHASSIS_SPEED_MODE_CMD 0x213F void chassis_speed_mode ( - Client session, + union Request* req, + uint16_t seq, + bool ack, float x, float y, float z ); diff --git a/include/connection.h b/include/connection.h new file mode 100644 index 0000000..3fd68df --- /dev/null +++ b/include/connection.h @@ -0,0 +1,13 @@ +#pragma once + +struct Connection; +typedef Connection* Connection; + +struct Connection* +conn_new ( + struct Connection* connection, + unsigned int source_port, + const char* source_ip, + unsigned int dest_port, + const char* dest_ip ); +extern const size_t SIZEOF_CONN; diff --git a/include/message.h b/include/message.h index d9e6cda..76eab84 100644 --- a/include/message.h +++ b/include/message.h @@ -5,7 +5,10 @@ #define PACKED __attribute__((__packed__)) -const uint8_t PREAMBLE = 0x55; +static const uint8_t CLIENT_HOST = 9; +static const uint8_t CLIENT_INDEX = 6; + +static const uint8_t PREAMBLE = 0x55; struct PACKED Header { @@ -261,61 +264,3 @@ enum MESSAGEERR { enum MESSAGEERR message_validate(const union Message* message); - -static inline uint8_t host2byte(uint8_t host, uint8_t index) { - return index * 32 + host; -} - -static inline void byte2host(uint8_t b, uint8_t* host, uint8_t* index) { - *host = (b & 0x1F); - *index = b >> 5; -} - -static -inline -size_t -message_length(uint16_t cmd) { - switch(cmd) { - case SET_SDK_CONNECTION_CMD: - return sizeof(struct SetSdkConnectionReq); - case SDK_HEARTBEAT_CMD: - return sizeof(struct SdkHeartbeatReq); - case SET_SDK_MODE_CMD: - return sizeof(struct SetSdkModeReq); - case SET_SYSTEM_LED_CMD: - return sizeof(struct SetSystemLedReq); - case SET_ROBOT_MODE_CMD: - return sizeof(struct SetRobotModeReq); - case SUBNODE_RESET_CMD: - return sizeof(struct SubNodeResetReq); - case SUBSCRIBE_ADD_NODE_CMD: - return sizeof(struct SubscribeAddNodeReq); - case SET_WHEEL_SPEED_CMD: - return sizeof(struct SetWheelSpeedReq); - case CHASSIS_SPEED_MODE_CMD: - return sizeof(struct ChassisSpeedModeReq); - default: - return 0; - } -} - -static -inline -uint8_t -message_module(uint16_t cmd) { - switch(cmd) { - case SET_SDK_CONNECTION_CMD: - case SDK_HEARTBEAT_CMD: - case SET_SDK_MODE_CMD: - case SET_SYSTEM_LED_CMD: - case SET_ROBOT_MODE_CMD: - case SUBNODE_RESET_CMD: - case SUBSCRIBE_ADD_NODE_CMD: - return host2byte(SDK_HOST, SDK_INDEX); - case SET_WHEEL_SPEED_CMD: - case CHASSIS_SPEED_MODE_CMD: - return host2byte(CHASSIS_HOST, CHASSIS_INDEX); - default: - return 0; - } -} diff --git a/include/robomaster.h b/include/robomaster.h index 9da532f..7bf2b61 100644 --- a/include/robomaster.h +++ b/include/robomaster.h @@ -12,3 +12,12 @@ Client client_new(); void client_connect(Client client); void poll_message(Client client, union Message* message); +static inline uint8_t host2byte(uint8_t host, uint8_t index) { + return index * 32 + host; +} + +static inline void byte2host(uint8_t b, uint8_t* host, uint8_t* index) { + *host = (b & 0x1F); + *index = b >> 5; +} + diff --git a/include/sdk.h b/include/sdk.h index d7db7f0..9291b91 100644 --- a/include/sdk.h +++ b/include/sdk.h @@ -8,7 +8,7 @@ static const uint8_t SDK_HOST = 9; static const uint8_t SDK_INDEX = 0; -static const uint16_t SET_SDK_CONNECTION_CMD = 0xD43F; +#define SET_SDK_CONNECTION_CMD 0xD43F enum CONNECTION { CONNECTION_WIFI_AP = 0, @@ -18,23 +18,31 @@ enum CONNECTION { void set_sdk_connection( - Client session, + union Request* req, + uint16_t seq, + bool ack, enum CONNECTION connection_type, uint32_t ip_address, uint16_t port ); -static const uint16_t SDK_HEARTBEAT_CMD = 0xD53F; +#define SDK_HEARTBEAT_CMD 0xD53F void sdk_heartbeat( - Client session ); + union Request* req, + uint16_t seq, + bool ack ); -static const uint16_t SET_SDK_MODE_CMD = 0xD13F; +#define SET_SDK_MODE_CMD 0xD13F void -set_sdk_mode(Client session, bool enable); +set_sdk_mode( + union Request* req, + uint16_t seq, + bool ack, + bool enable ); -static const uint16_t SET_SYSTEM_LED_CMD = 0x333F; +#define SET_SYSTEM_LED_CMD 0x333F enum LEDCOMP { LEDCOMP_BOTTOM_BACK = 0x1, @@ -57,7 +65,9 @@ enum LEDEFFECT { }; void set_system_led ( - Client session, + union Request* req, + uint16_t seq, + bool ack, uint8_t red, uint8_t green, uint8_t blue, @@ -67,7 +77,7 @@ void set_system_led ( uint16_t t1, uint16_t t2 ); -static const uint16_t SET_ROBOT_MODE_CMD = 0x463F; +#define SET_ROBOT_MODE_CMD 0x463F enum MOVEMENTMODE { MOVEMENTMODE_FREE, @@ -77,18 +87,24 @@ enum MOVEMENTMODE { void set_robot_mode ( - Client session, + union Request* req, + uint16_t seq, + bool ack, enum MOVEMENTMODE mode ); -static const uint16_t SUBNODE_RESET_CMD = 0x0248; +#define SUBNODE_RESET_CMD 0x0248 void subnode_reset ( - Client session ); + union Request* req, + uint16_t seq, + bool ack ); -static const uint16_t SUBSCRIBE_ADD_NODE_CMD = 0x0148; +#define SUBSCRIBE_ADD_NODE_CMD 0x0148 void subscribe_add_node ( - Client session ); + union Request* req, + uint16_t seq, + bool ack ); diff --git a/src/client.h b/src/client.h index b57ac9b..dacf8d4 100644 --- a/src/client.h +++ b/src/client.h @@ -4,9 +4,7 @@ struct Client { - uint8_t hostbyte; - - int16_t seq; + uint16_t seq; union { struct { @@ -18,6 +16,3 @@ struct Client { }; -static const uint8_t CLIENT_HOST = 9; -static const uint8_t CLIENT_INDEX = 6; - diff --git a/src/connection.c b/src/connection.c index 2b79b25..c9ad568 100644 --- a/src/connection.c +++ b/src/connection.c @@ -1,4 +1,4 @@ -#include "message.h" +#include "robomaster.h" #include "client.h" #include "crc.h" #include "connection.h" @@ -12,6 +12,55 @@ #include #include +static +inline +size_t +message_length(int cmd) { + switch(cmd) { + case SET_SDK_CONNECTION_CMD: + return sizeof(struct SetSdkConnectionReq); + case SDK_HEARTBEAT_CMD: + return sizeof(struct SdkHeartbeatReq); + case SET_SDK_MODE_CMD: + return sizeof(struct SetSdkModeReq); + case SET_SYSTEM_LED_CMD: + return sizeof(struct SetSystemLedReq); + case SET_ROBOT_MODE_CMD: + return sizeof(struct SetRobotModeReq); + case SUBNODE_RESET_CMD: + return sizeof(struct SubNodeResetReq); + case SUBSCRIBE_ADD_NODE_CMD: + return sizeof(struct SubscribeAddNodeReq); + case SET_WHEEL_SPEED_CMD: + return sizeof(struct SetWheelSpeedReq); + case CHASSIS_SPEED_MODE_CMD: + return sizeof(struct ChassisSpeedModeReq); + default: + return 0; + } +} + +static +inline +uint8_t +message_module(int cmd) { + switch(cmd) { + case SET_SDK_CONNECTION_CMD: + case SDK_HEARTBEAT_CMD: + case SET_SDK_MODE_CMD: + case SET_SYSTEM_LED_CMD: + case SET_ROBOT_MODE_CMD: + case SUBNODE_RESET_CMD: + case SUBSCRIBE_ADD_NODE_CMD: + return host2byte(SDK_HOST, SDK_INDEX); + case SET_WHEEL_SPEED_CMD: + case CHASSIS_SPEED_MODE_CMD: + return host2byte(CHASSIS_HOST, CHASSIS_INDEX); + default: + return 0; + } +} + // The greated file descriptor is needed for polling the sockets. // It needs to be global for the whole process. int max_fd = -1; @@ -72,8 +121,8 @@ connection_poll_ready(struct Client* client) { if(client->conns[i]) FD_SET(client->conns[i]->sockfd, &read_fds); - //struct timeval timeout = {0, 0}; - int result = select(max_fd + 1, &read_fds, NULL, NULL, NULL); + struct timeval timeout = {0, 0}; + int result = select(max_fd + 1, &read_fds, NULL, NULL, &timeout); // Check for socket polling errors if(result < 0) { @@ -118,26 +167,24 @@ connection_read(struct Connection* conn, union Message* resp) { } void -req_send(struct Connection* conn, union Request* req, size_t length) { +req_send(struct Connection* conn, union Request* req) { if(!conn || !req) return; + size_t length = message_length(req->header.cmd); sendto(conn->sockfd, req, length, 0, (struct sockaddr*)&conn->remote_addr, conn->addrlen); } void -req_finalize(struct Client* client, uint8_t cmdset, uint8_t cmdid, uint8_t hostbyte, bool need_ack, size_t length, union Request* req) { +req_finalize(uint16_t seq, uint16_t cmd, bool need_ack, union Request* req) { + size_t length = message_length(cmd); req->header.preamble = 0x55; - //req->header.length = length & 0x1FFF | 0x400; - req->header.length = length; + req->header.length = (length & 0x1FFF) | 0x400; req->header.crc = crc8(req, 3); - req->header.seq_id = client->seq++; - req->header.sender = client->hostbyte; - // TODO: Figure out what this is supposed to be - //req->header.receiver = host2byte(DEFAULT_CLIENT_HOST, DEFAULT_ROBOT_INDEX); - req->header.receiver = hostbyte; + req->header.seq_id = seq; + req->header.sender = host2byte(CLIENT_HOST, CLIENT_INDEX); + req->header.receiver = message_module(cmd); req->header.ack_needed = need_ack; - req->header.cmdset = cmdset; - req->header.cmdid = cmdid; + req->header.cmd = cmd; struct Footer* footer = (void*)req + length - sizeof(struct Footer); uint16_t crc = crc16(req, length - sizeof(struct Footer)); diff --git a/src/connection.h b/src/connection.h index 80196c1..5a2114e 100644 --- a/src/connection.h +++ b/src/connection.h @@ -22,8 +22,8 @@ void connection_read(struct Connection* connection, union Message* resp); void -req_finalize(struct Client* client, uint8_t cmdset, uint8_t cmdid, uint8_t hostbyte, bool need_ack, size_t length, union Request* req); +req_finalize(uint16_t seq, uint16_t cmd, bool need_ack, union Request* req); void -req_send(struct Connection* conn, union Request* req, size_t length); +req_send(struct Connection* conn, union Request* req); diff --git a/src/message.c b/src/message.c index 50aab48..2616b3d 100644 --- a/src/message.c +++ b/src/message.c @@ -4,7 +4,7 @@ enum MESSAGEERR message_validate(const union Message* message) { - uint16_t length = message->header.length; + uint16_t length = message->header.length & 0x1FFF & ~0x400; if(message->header.crc != crc8(message, 3)) return MESSAGEERR_HEADERCRC; diff --git a/src/modules/chassis.c b/src/modules/chassis.c index 876a709..55a8dde 100644 --- a/src/modules/chassis.c +++ b/src/modules/chassis.c @@ -4,30 +4,30 @@ void set_wheel_speed ( - Client session, + union Request* req, + uint16_t seq, + bool ack, int16_t w1, int16_t w2, int16_t w3, int16_t w4 ) { - union Request req = {0}; - req.wheel.wheel_speed[0] = w1; - req.wheel.wheel_speed[1] = w2; - req.wheel.wheel_speed[2] = w3; - req.wheel.wheel_speed[3] = w4; - req_finalize(session, 0x3F, SET_WHEEL_SPEED_CMDID, host2byte(CHASSIS_HOST, CHASSIS_INDEX), false, sizeof(struct SetWheelSpeedReq), &req); - req_send(session->dev_conn, &req, sizeof(struct SetWheelSpeedReq)); + req->wheel.wheel_speed[0] = w1; + req->wheel.wheel_speed[1] = w2; + req->wheel.wheel_speed[2] = w3; + req->wheel.wheel_speed[3] = w4; + req_finalize(seq, SET_WHEEL_SPEED_CMD, ack, req); } void chassis_speed_mode ( - Client session, + union Request* req, + uint16_t seq, + bool ack, float x, float y, float z ) { - union Request req = {0}; - req.chsspeed.speed[0] = x; - req.chsspeed.speed[1] = y; - req.chsspeed.speed[2] = z; - req_finalize(session, 0x3F, CHASSIS_SPEED_MODE_CMDID, host2byte(CHASSIS_HOST, CHASSIS_INDEX), false, sizeof(struct ChassisSpeedModeReq), &req); - req_send(session->dev_conn, &req, sizeof(struct ChassisSpeedModeReq)); + req->chsspeed.speed[0] = x; + req->chsspeed.speed[1] = y; + req->chsspeed.speed[2] = z; + req_finalize(seq, CHASSIS_SPEED_MODE_CMD, ack, req); } diff --git a/src/modules/sdk.c b/src/modules/sdk.c index 3e59984..efef707 100644 --- a/src/modules/sdk.c +++ b/src/modules/sdk.c @@ -4,72 +4,74 @@ void set_sdk_connection( - Client session, + union Request* req, + uint16_t seq, + bool ack, enum CONNECTION connection_type, uint32_t ip_address, uint16_t port ) { - union Request req = {0}; - req.sdkconn.control = 0; - req.sdkconn.host = session->hostbyte; - req.sdkconn.connection = connection_type; - req.sdkconn.protocol = 0; - req.sdkconn.ip_address = ip_address; - req.sdkconn.port = port; - req_finalize(session, 0x3F, SET_SDK_CONNECTION_CMDID, host2byte(SDK_HOST, SDK_INDEX), true, sizeof(struct SetSdkConnectionReq), &req); - req_send(session->sdk_conn, &req, sizeof(struct SetSdkConnectionReq)); + req->sdkconn.control = 0; + req->sdkconn.host = host2byte(CLIENT_HOST, CLIENT_INDEX); + req->sdkconn.connection = connection_type; + req->sdkconn.protocol = 0; + req->sdkconn.ip_address = ip_address; + req->sdkconn.port = port; + req_finalize(seq, SET_SDK_CONNECTION_CMD, ack, req); } void set_sdk_mode( - Client session, + union Request* req, + uint16_t seq, + bool ack, bool enable ) { - union Request req = {0}; - req.sdkmode.enable = enable; - req_finalize(session, 0x3F, SET_SDK_MODE_CMDID, host2byte(SDK_HOST, SDK_INDEX), true, sizeof(struct SetSdkModeReq), &req); - req_send(session->dev_conn, &req, sizeof(struct SetSdkModeReq)); + req->sdkmode.enable = enable; + req_finalize(seq, SET_SDK_MODE_CMD, ack, req); } void sdk_heartbeat( - Client session ) { - union Request req = {0}; - req_finalize(session, 0x3F, SDK_HEARTBEAT_CMDID, host2byte(SDK_HOST, SDK_INDEX), false, sizeof(struct SdkHeartbeatReq), &req); - req_send(session->dev_conn, &req, sizeof(struct SdkHeartbeatReq)); + union Request* req, + uint16_t seq, + bool ack ) { + req_finalize(seq, SDK_HEARTBEAT_CMD, ack, req); } void set_robot_mode ( - Client session, + union Request* req, + uint16_t seq, + bool ack, enum MOVEMENTMODE mode ) { - union Request req = {0}; - req.mvmode.mode = mode; - req_finalize(session, 0x3F, SET_ROBOT_MODE_CMDID, host2byte(SDK_HOST, SDK_INDEX), true, sizeof(struct SetRobotModeReq), &req); - req_send(session->dev_conn, &req, sizeof(struct SetRobotModeReq)); + req->mvmode.mode = mode; + req_finalize(seq, SET_ROBOT_MODE_CMD, ack, req); } void subnode_reset ( - Client session ) { - union Request req = {0}; - req.subnodereset.hostbyte = session->hostbyte; - req_finalize(session, 0x48, SUBNODE_RESET_CMDID, host2byte(SDK_HOST, SDK_INDEX), true, sizeof(struct SubNodeResetReq), &req); - req_send(session->dev_conn, &req, sizeof(struct SubNodeResetReq)); + union Request* req, + uint16_t seq, + bool ack ) { + req->subnodereset.hostbyte = host2byte(CLIENT_HOST, CLIENT_INDEX); + req_finalize(seq, SUBNODE_RESET_CMD, ack, req); } void subscribe_add_node ( - Client session ) { - union Request req = {0}; - req.subnodeadd.hostbyte = session->hostbyte; - req.subnodeadd.sub_vision = 0x03000000; - req_finalize(session, 0x48, SUBSCRIBE_ADD_NODE_CMDID, host2byte(SDK_HOST, SDK_INDEX), true, sizeof(struct SubscribeAddNodeReq), &req); - req_send(session->dev_conn, &req, sizeof(struct SubscribeAddNodeReq)); + union Request* req, + uint16_t seq, + bool ack ) { + req->subnodeadd.hostbyte = host2byte(CLIENT_HOST, CLIENT_INDEX); + req->subnodeadd.sub_vision = 0x03000000; + req_finalize(seq, SUBSCRIBE_ADD_NODE_CMD, ack, req); } void set_system_led ( - Client session, + union Request* req, + uint16_t seq, + bool ack, uint8_t red, uint8_t green, uint8_t blue, @@ -79,20 +81,18 @@ set_system_led ( uint16_t t1, uint16_t t2 ) { - union Request req = {0}; - req.led.comp_mask = comp; - req.led.led_mask = led_mask; - req.led.effect_mode = effect; - req.led.control_mode = 7; - req.led.red = red; - req.led.green = green; - req.led.blue = blue; - req.led.loop = 0; - req.led.t1 = t1; - req.led.t2 = t2; + req->led.comp_mask = comp; + req->led.led_mask = led_mask; + req->led.effect_mode = effect; + req->led.control_mode = 7; + req->led.red = red; + req->led.green = green; + req->led.blue = blue; + req->led.loop = 0; + req->led.t1 = t1; + req->led.t2 = t2; - req_finalize(session, 0x3F, SET_SYSTEM_LED_CMDID, host2byte(SDK_HOST, SDK_INDEX), true, sizeof(struct SetSystemLedReq), &req); - req_send(session->dev_conn, &req, sizeof(struct SetSystemLedReq)); + req_finalize(seq, SET_SYSTEM_LED_CMD, ack, req); } diff --git a/src/robo.c b/src/robo.c new file mode 100644 index 0000000..501870a --- /dev/null +++ b/src/robo.c @@ -0,0 +1,248 @@ +#include "roboeasy.h" +#include "robomaster.h" +#include "connection.h" + +#include +#include + +struct Robot { + + struct Client* client; + uint16_t seq; + + int16_t wheels[4]; + bool dirty_wheels; + + uint8_t colors[3]; + bool dirty_colors; + + 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_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_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_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; + } + 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 Robot* robot = malloc(sizeof(struct Robot)); + memset(robot, 0, sizeof(struct Robot)); + 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; +} diff --git a/src/robomaster.c b/src/robomaster.c index 951c042..f272259 100644 --- a/src/robomaster.c +++ b/src/robomaster.c @@ -11,7 +11,6 @@ Client client_new() { struct Client* client = malloc(sizeof(struct Client)); memset(client, 0, sizeof(struct Client)); // TODO: Make this configurable - client->hostbyte = host2byte(CLIENT_HOST, CLIENT_INDEX); return client; } @@ -19,8 +18,6 @@ void client_connect(Client client) { client->sdk_conn = connection_new(0, 0, 30030, "192.168.2.1"); // TODO: This should probably use the source IP in the response body client->dev_conn = connection_new(10010, "192.168.2.24", 20020, "192.168.2.1"); - - set_sdk_connection(client, CONNECTION_WIFI_AP, 0, 10010); } void poll_message(Client client, union Message* resp) { @@ -32,5 +29,7 @@ void poll_message(Client client, union Message* resp) { void drive(Client client, float x, float y, float r) { float w1 = y + x + r, w2 = y - x -r, w3 = y + x -r, w4 = y - x + r; - set_wheel_speed(client, w1 * 1000, -w2 * 1000, -w3 * 1000, w4 * 1000); + union Request req; + set_wheel_speed(&req, client->seq, false, w1 * 1000, -w2 * 1000, -w3 * 1000, w4 * 1000); + req_send(client->dev_conn, &req); } diff --git a/src/sdl.c b/src/sdl.c index 2b855fc..ab27553 100644 --- a/src/sdl.c +++ b/src/sdl.c @@ -1,13 +1,35 @@ -#include "robomaster.h" +#include "roboeasy.h" #include "SDL2/SDL.h" #include #include +#include + +struct { + unsigned char r; + unsigned char g; + unsigned char b; +} const colors[] = { + {0xFF, 0x00, 0x00}, + {0x00, 0xFF, 0x00}, + {0x00, 0x00, 0xFF} +}; +int color = 0; + +float x = 0, y = 0, z = 0; +static Uint32 drive_timer_handler(Uint32 interval, void* param) { + robot_drive((Robot)param, x, y, z); + return 75; +} +static Uint32 heartbeat_timer_handler(Uint32 interval, void* param) { + robot_heartbeat((Robot)param); + return 1000; +} int main(int argc, char* argv[]) { if(SDL_Init(SDL_INIT_VIDEO | SDL_INIT_GAMECONTROLLER) < 0) { fprintf(stderr, "%s", SDL_GetError()); - return 1; + return 1; } SDL_Window* win = SDL_CreateWindow( "Robomaster", @@ -17,51 +39,15 @@ int main(int argc, char* argv[]) { SDL_WINDOW_RESIZABLE ); if(!win) { fprintf(stderr, "%s", SDL_GetError()); - return 1; - } - - Client client = client_new(); - client_connect(client); - - union Message msg; - poll_message(client, &msg); - if(msg.header.cmdid != SET_SDK_CONNECTION_CMDID || msg.resp.sdkconn.retcode) { - fprintf(stderr, "Could not set SDK connection\n"); - return 1; - } - - set_sdk_mode(client, true); - poll_message(client, &msg); - if(msg.header.cmdid != SET_SDK_MODE_CMDID || msg.resp.sdkmode.retcode) { - fprintf(stderr, "Could not set SDK mode\n"); return 1; } - subnode_reset(client); - poll_message(client, &msg); - if(msg.header.cmdid != SUBNODE_RESET_CMDID || msg.resp.subnodereset.retcode) { - fprintf(stderr, "Could not reset subnode subscription\n"); - return 1; - } - - subscribe_add_node(client); - poll_message(client, &msg); - if(msg.header.cmdid != SUBSCRIBE_ADD_NODE_CMDID || (msg.resp.subnodeadd.retcode && msg.resp.subnodeadd.retcode != 0x50)) { - fprintf(stderr, "Could not subscribe node\n"); - return 1; - } + Robot robot = robot_new(); + robot_init(robot); + SDL_AddTimer(75, drive_timer_handler, robot); + SDL_AddTimer(1000, heartbeat_timer_handler, robot); - set_robot_mode(client, MOVEMENTMODE_FREE); - poll_message(client, &msg); - if(msg.header.cmdid != SET_ROBOT_MODE_CMDID || msg.resp.mvmode.retcode) { - fprintf(stderr, "Could not set move mode\n"); - return 1; - } - - bool quit = false; - Uint32 time = SDL_GetTicks(); - float x = 0, y = 0, z = 0; - while(!quit) { + while(robot_work(robot)) { SDL_Event event; while(SDL_PollEvent(&event)) { switch(event.type) { @@ -69,51 +55,39 @@ int main(int argc, char* argv[]) { case SDL_KEYDOWN: switch(event.key.keysym.scancode) { case SDL_SCANCODE_Q: - z = event.type == SDL_KEYUP ? 0 : 250; + z = event.type == SDL_KEYUP ? 0 : 0.25; break; case SDL_SCANCODE_E: - z = event.type == SDL_KEYUP ? 0 : -250; + z = event.type == SDL_KEYUP ? 0 : -0.25; break; case SDL_SCANCODE_LEFT: case SDL_SCANCODE_A: - x = event.type == SDL_KEYUP ? 0 : 250; + x = event.type == SDL_KEYUP ? 0 : 0.25; break; case SDL_SCANCODE_RIGHT: case SDL_SCANCODE_D: - x = event.type == SDL_KEYUP ? 0 : -250; + x = event.type == SDL_KEYUP ? 0 : -0.25; break; case SDL_SCANCODE_UP: case SDL_SCANCODE_W: - y = event.type == SDL_KEYUP ? 0 : 250; + y = event.type == SDL_KEYUP ? 0 : 0.25; break; case SDL_SCANCODE_DOWN: case SDL_SCANCODE_S: - y = event.type == SDL_KEYUP ? 0 : -250; + y = event.type == SDL_KEYUP ? 0 : -0.25; break; + case SDL_SCANCODE_SPACE: + robot_led(robot, colors[color].r, colors[color].g, colors[color].b); + color = (color + 1) % 3; default: break; - } + } case SDL_WINDOWEVENT: if(event.window.event != SDL_WINDOWEVENT_CLOSE) break; case SDL_QUIT: - quit = true; + robot_stop(robot); default: break; + } } - } - Uint32 currtime = SDL_GetTicks(); - if(currtime - time >= 75) - { - time = currtime; - sdk_heartbeat(client); - float w1 = y + x + z, w2 = y - x -z, w3 = y + x -z, w4 = y - x + z; - set_wheel_speed(client, w1, -w2, -w3, w4); - } - } - - set_sdk_mode(client, false); - poll_message(client, &msg); - if(msg.header.cmdid != SET_SDK_MODE_CMDID || msg.resp.sdkmode.retcode) { - fprintf(stderr, "Could not disable SDK mode\n"); - return 1; } SDL_Quit();