Control robot with WASD keys

src/modules/sdk.c

@@ -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

@@ -45,11 +45,11 @@ int main(int argc, char* argv[])
     }
 
     sdk_heartbeat(client);
-    poll_message(client, &msg);
+    //poll_message(client, &msg);
-    if(msg.header.cmdid != SDK_HEARTBEAT_CMDID || msg.resp.heartbeat.retcode) {
+    //if(msg.header.cmdid != SDK_HEARTBEAT_CMDID || msg.resp.heartbeat.retcode) {
-        fprintf(stderr, "Did not receive heartbeat\n");
+    //    fprintf(stderr, "Did not receive heartbeat\n");
-        return 1;
+    //    return 1;
-    }
+    //}
 
     int c;
     while((c = getopt(argc, argv, "hl:::s:::")) != -1) {

src/sdl.c

@@ -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();