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

Control robot with WASD keys

Browse Source
This commit is contained in:
pgsocks
2023-02-02 01:14:29 -06:00
parent 2b0b5e7b4f
commit 8a34d18eac
3 changed files with 87 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sdk.c
+1 -1
View File
@@ -34,7 +34,7 @@ void
sdk_heartbeat(
Client session ) {
union Request req = {0};
req_finalize(session, 0x3F, SDK_HEARTBEAT_CMDID, host2byte(SDK_HOST, SDK_INDEX), true, sizeof(struct SdkHeartbeatReq), &req);
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));
}
src/robomastersh.c
+5 -5
View File
@@ -45,11 +45,11 @@ int main(int argc, char* argv[])
}
sdk_heartbeat(client);
poll_message(client, &msg);
if(msg.header.cmdid != SDK_HEARTBEAT_CMDID || msg.resp.heartbeat.retcode) {
fprintf(stderr, "Did not receive heartbeat\n");
return 1;
}
//poll_message(client, &msg);
//if(msg.header.cmdid != SDK_HEARTBEAT_CMDID || msg.resp.heartbeat.retcode) {
// fprintf(stderr, "Did not receive heartbeat\n");
// return 1;
//}
int c;
while((c = getopt(argc, argv, "hl:::s:::")) != -1) {
src/sdl.c
+81
View File
@@ -20,17 +20,98 @@ int main(int argc, char* argv[]) {
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;
}
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) {
SDL_Event event;
while(SDL_PollEvent(&event)) {
switch(event.type) {
case SDL_KEYUP:
case SDL_KEYDOWN:
switch(event.key.keysym.scancode) {
case SDL_SCANCODE_LEFT:
case SDL_SCANCODE_A:
x = event.type == SDL_KEYUP ? 0 : -1;
break;
case SDL_SCANCODE_RIGHT:
case SDL_SCANCODE_D:
x = event.type == SDL_KEYUP ? 0 : 1;
break;
case SDL_SCANCODE_UP:
case SDL_SCANCODE_W:
y = event.type == SDL_KEYUP ? 0 : 1;
break;
case SDL_SCANCODE_DOWN:
case SDL_SCANCODE_S:
y = event.type == SDL_KEYUP ? 0 : -1;
break;
default: break;
}
case SDL_WINDOWEVENT:
if(event.window.event != SDL_WINDOWEVENT_CLOSE) break;
case SDL_QUIT:
quit = true;
default: break;
}
}
Uint32 currtime = SDL_GetTicks();
if(currtime - time >= 75)
{
time = currtime;
sdk_heartbeat(client);
//poll_message(client, &msg);
//if(msg.header.cmdid != SDK_HEARTBEAT_CMDID || msg.resp.heartbeat.retcode) {
// fprintf(stderr, "Did not receive heartbeat\n");
// return 1;
//}
chassis_speed_mode(client, x, y, z );
}
}
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();