pgsocks
/
robomaster-sdk
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Add more messages

Browse Source
refactor
PgSocks 2 years ago
parent 07a0d7eca7
commit 1eca1b25e7
4 changed files with 223 additions and 11 deletions
Show Stats Download Patch File Download Diff File

82
include/message.h
Unescape Escape View File

@ -92,6 +92,78 @@ struct PACKED SetSystemLedResp {
struct Footer footer; 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 PACKED SetWheelSpeedReq
{ {
struct Header header; struct Header header;
@ -143,6 +215,11 @@ union Request {
struct SetSdkModeReq sdkmode; struct SetSdkModeReq sdkmode;
struct SetSystemLedReq led; struct SetSystemLedReq led;
struct SetWheelSpeedReq wheel; struct SetWheelSpeedReq wheel;
struct SetChassisWheelSpeedReq chswheel;
struct ChassisSpeedModeReq chsspeed;
struct SetRobotModeReq mvmode;
struct SubNodeResetReq subnodereset;
struct SubscribeAddNodeReq subnodeadd;
}; };
union Response { union Response {
struct Header header; struct Header header;
@ -150,6 +227,11 @@ union Response {
struct SetSdkModeResp sdkmode; struct SetSdkModeResp sdkmode;
struct SetSystemLedResp led; struct SetSystemLedResp led;
struct SetWheelSpeedResp wheel; struct SetWheelSpeedResp wheel;
struct SetChassisWheelSpeedResp chswheel;
struct ChassisSpeedModeResp chsspeed;
struct SetRobotModeResp mvmode;
struct SubNodeResetResp subnodereset;
struct SubscribeAddNodeResp subnodeadd;
}; };
union Message { union Message {
struct Header header; struct Header header;

46
include/wheel.h
Unescape Escape View File

@ -14,3 +14,49 @@ set_wheel_speed (
int16_t w2, int16_t w2,
int16_t w3, int16_t w3,
int16_t w4 ); 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 );

61
src/messages/set_wheel_speed.c
Unescape Escape View File

@ -15,5 +15,64 @@ set_wheel_speed (
req.wheel.wheel_speed[2] = w3; req.wheel.wheel_speed[2] = w3;
req.wheel.wheel_speed[3] = w4; req.wheel.wheel_speed[3] = w4;
req_finalize(session, 0x3F, SET_WHEEL_SPEED_CMDID, sizeof(struct SetWheelSpeedReq), &req); 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));
} }

45
src/robomastersh.c
Unescape Escape View File

@ -18,13 +18,34 @@ int main(int argc, char* argv[])
set_sdk_mode(client, true); set_sdk_mode(client, true);
poll_message(client, &msg); 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"); fprintf(stderr, "Could not set SDK mode\n");
return 1; 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; int c;
while((c = getopt(argc, argv, "hl:::w::::"))) { while((c = getopt(argc, argv, "hl:::s:::")) != -1) {
switch(c) { switch(c) {
case 'l': case 'l':
set_system_led ( set_system_led (
@ -38,25 +59,29 @@ int main(int argc, char* argv[])
100, 100,
100 ); 100 );
poll_message(client, &msg); 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"); fprintf(stderr, "Could not set LED color\n");
return 1; return 1;
} }
break; break;
case 'w': case 's':
set_wheel_speed ( chassis_speed_mode (
client, client,
strtol(argv[optind + 0], NULL, 0), atof(argv[optind + 0]),
strtol(argv[optind + 1], NULL, 0), atof(argv[optind + 1]),
strtol(argv[optind + 2], NULL, 0), atof(argv[optind + 2]) );
strtol(argv[optind + 3], NULL, 0) ); 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; break;
case '?': case '?':
default: default:
fprintf(stderr, "Unknown argument %c\n", optopt); fprintf(stderr, "Unknown argument %c\n", optopt);
case 'h': case 'h':
printf("Usage: [-h] [-l r g b] [-w w1 w2 w3 w4]"); printf("Usage: [-h] [-l r g b] [-w w1 w2 w3 w4]");
break; return 0;
} }
} }

Write Preview
Loading…
Cancel
Save