From 1eca1b25e765747431791b7cc3e1bf9dca838243 Mon Sep 17 00:00:00 2001 From: PgSocks Date: Sun, 1 Jan 2023 00:15:46 -0600 Subject: [PATCH] Add more messages --- include/message.h | 82 ++++++++++++++++++++++++++++++++++ include/wheel.h | 46 +++++++++++++++++++ src/messages/set_wheel_speed.c | 61 ++++++++++++++++++++++++- src/robomastersh.c | 45 ++++++++++++++----- 4 files changed, 223 insertions(+), 11 deletions(-) diff --git a/include/message.h b/include/message.h index 184beaf..92b8934 100644 --- a/include/message.h +++ b/include/message.h @@ -92,6 +92,78 @@ struct PACKED SetSystemLedResp { struct Footer footer; }; +struct PACKED SubscribeAddNodeReq +{ + struct Header header; + uint8_t hostbyte; + uint32_t sub_vision; // always 0x03000000 + struct Footer footer; +}; + +struct PACKED SubscribeAddNodeResp +{ + struct Header header; + uint8_t retcode; // 0x00 or 0x50 are acceptable here + uint8_t hostbyte; + struct Footer footer; +}; + +struct PACKED SubNodeResetReq +{ + struct Header header; + uint8_t hostbyte; + struct Footer footer; +}; + +struct PACKED SubNodeResetResp +{ + struct Header header; + uint8_t retcode; + struct Footer footer; +}; + +struct PACKED SetRobotModeReq +{ + struct Header header; + uint8_t mode; + struct Footer footer; +}; + +struct PACKED SetRobotModeResp +{ + struct Header header; + uint8_t retcode; + struct Footer footer; +}; + +struct PACKED ChassisSpeedModeReq +{ + struct Header header; + float speed[3]; + struct Footer footer; +}; + +struct PACKED ChassisSpeedModeResp +{ + struct Header header; + int8_t retcode; + struct Footer footer; +}; + +struct PACKED SetChassisWheelSpeedReq +{ + struct Header header; + int8_t wheel_speed[4]; + struct Footer footer; +}; + +struct PACKED SetChassisWheelSpeedResp +{ + struct Header header; + int8_t retcode; + struct Footer footer; +}; + struct PACKED SetWheelSpeedReq { struct Header header; @@ -143,6 +215,11 @@ union Request { struct SetSdkModeReq sdkmode; struct SetSystemLedReq led; struct SetWheelSpeedReq wheel; + struct SetChassisWheelSpeedReq chswheel; + struct ChassisSpeedModeReq chsspeed; + struct SetRobotModeReq mvmode; + struct SubNodeResetReq subnodereset; + struct SubscribeAddNodeReq subnodeadd; }; union Response { struct Header header; @@ -150,6 +227,11 @@ union Response { struct SetSdkModeResp sdkmode; struct SetSystemLedResp led; struct SetWheelSpeedResp wheel; + struct SetChassisWheelSpeedResp chswheel; + struct ChassisSpeedModeResp chsspeed; + struct SetRobotModeResp mvmode; + struct SubNodeResetResp subnodereset; + struct SubscribeAddNodeResp subnodeadd; }; union Message { struct Header header; diff --git a/include/wheel.h b/include/wheel.h index ff4caca..086ca01 100644 --- a/include/wheel.h +++ b/include/wheel.h @@ -14,3 +14,49 @@ set_wheel_speed ( int16_t w2, int16_t w3, int16_t w4 ); + +static const uint8_t SET_CHASSIS_WHEEL_SPEED_CMDID = 0x26; + +void +set_chassis_wheel_speed ( + Client session, + int8_t w1, + int8_t w2, + int8_t w3, + int8_t w4 ); + +static const uint8_t CHASSIS_SPEED_MODE_CMDID = 0x21; + +void +chassis_speed_mode ( + Client session, + float x, + float y, + float z ); + +static const uint8_t SET_ROBOT_MODE_CMDID = 0x46; + +enum MOVEMENTMODE { + MOVEMENTMODE_FREE, + MOVEMENTMODE_GIMBAL_LEAD, + MOVEMENTMODE_CHASSIS_LEAD +}; + +void +set_robot_mode ( + Client session, + enum MOVEMENTMODE mode ); + +// cmdset 0x48 +static const uint8_t SUBNODE_RESET_CMDID = 0x02; + +void +subnode_reset ( + Client session ); + +// cmdset 0x48 +static const uint8_t SUBSCRIBE_ADD_NODE_CMDID = 0x01; + +void +subscribe_add_node ( + Client session ); diff --git a/src/messages/set_wheel_speed.c b/src/messages/set_wheel_speed.c index 724b9fb..6cbc737 100644 --- a/src/messages/set_wheel_speed.c +++ b/src/messages/set_wheel_speed.c @@ -15,5 +15,64 @@ set_wheel_speed ( req.wheel.wheel_speed[2] = w3; req.wheel.wheel_speed[3] = w4; req_finalize(session, 0x3F, SET_WHEEL_SPEED_CMDID, sizeof(struct SetWheelSpeedReq), &req); - req_send(session->sdk_conn, &req, sizeof(struct SetWheelSpeedReq)); + req_send(session->dev_conn, &req, sizeof(struct SetWheelSpeedReq)); +} + +void +set_chassis_wheel_speed ( + Client session, + int8_t w1, + int8_t w2, + int8_t w3, + int8_t w4 ) { + union Request req = {0}; + req.chswheel.wheel_speed[0] = w1; + req.chswheel.wheel_speed[1] = w2; + req.chswheel.wheel_speed[2] = w3; + req.chswheel.wheel_speed[3] = w4; + req_finalize(session, 0x3F, SET_CHASSIS_WHEEL_SPEED_CMDID, sizeof(struct SetChassisWheelSpeedReq), &req); + req_send(session->dev_conn, &req, sizeof(struct SetChassisWheelSpeedReq)); +} + +void +chassis_speed_mode ( + Client session, + 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, sizeof(struct ChassisSpeedModeReq), &req); + req_send(session->dev_conn, &req, sizeof(struct ChassisSpeedModeReq)); +} + +void +set_robot_mode ( + Client session, + enum MOVEMENTMODE mode ) { + union Request req = {0}; + req.mvmode.mode = mode; + req_finalize(session, 0x3F, SET_ROBOT_MODE_CMDID, sizeof(struct SetRobotModeReq), &req); + req_send(session->dev_conn, &req, sizeof(struct SetRobotModeReq)); +} + +void +subnode_reset ( + Client session ) { + union Request req = {0}; + req.subnodereset.hostbyte = session->hostbyte; + req_finalize(session, 0x48, SUBNODE_RESET_CMDID, sizeof(struct SubNodeResetReq), &req); + req_send(session->dev_conn, &req, sizeof(struct SubNodeResetReq)); +} + +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, sizeof(struct SubscribeAddNodeReq), &req); + req_send(session->dev_conn, &req, sizeof(struct SubscribeAddNodeReq)); } diff --git a/src/robomastersh.c b/src/robomastersh.c index 99ee094..2272b5e 100644 --- a/src/robomastersh.c +++ b/src/robomastersh.c @@ -18,13 +18,34 @@ int main(int argc, char* argv[]) set_sdk_mode(client, true); poll_message(client, &msg); - if(msg.header.cmdid == SET_SDK_MODE_CMDID || msg.resp.sdkmode.retcode) { + 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; + } + + 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; + } + int c; - while((c = getopt(argc, argv, "hl:::w::::"))) { + while((c = getopt(argc, argv, "hl:::s:::")) != -1) { switch(c) { case 'l': set_system_led ( @@ -38,25 +59,29 @@ int main(int argc, char* argv[]) 100, 100 ); poll_message(client, &msg); - if(msg.header.cmdid == SET_SYSTEM_LED_CMDID || msg.resp.led.retcode) { + if(msg.header.cmdid != SET_SYSTEM_LED_CMDID || msg.resp.led.retcode) { fprintf(stderr, "Could not set LED color\n"); return 1; } break; - case 'w': - set_wheel_speed ( + case 's': + chassis_speed_mode ( client, - strtol(argv[optind + 0], NULL, 0), - strtol(argv[optind + 1], NULL, 0), - strtol(argv[optind + 2], NULL, 0), - strtol(argv[optind + 3], NULL, 0) ); + atof(argv[optind + 0]), + atof(argv[optind + 1]), + atof(argv[optind + 2]) ); + poll_message(client, &msg); + if(msg.header.cmdid != CHASSIS_SPEED_MODE_CMDID || msg.resp.chsspeed.retcode) { + fprintf(stderr, "Could not set speed\n"); + return 1; + } break; case '?': default: fprintf(stderr, "Unknown argument %c\n", optopt); case 'h': printf("Usage: [-h] [-l r g b] [-w w1 w2 w3 w4]"); - break; + return 0; } }