Merge pull request #11 from wuhanstudio/add-structure

Add controller structure

Author: yqiu2018

.gitignore

@@ -50,4 +50,8 @@ modules.order
 Module.symvers
 Mkfile.old
 dkms.conf
+<<<<<<< HEAD
+
+=======
 .vscode/
+>>>>>>> add-structure

chassis/chassis.c

@@ -91,3 +91,47 @@ rt_err_t chassis_update(chassis_t chas)
     }
     return RT_EOK;
 }
+
+rt_err_t chassis_parse_command(chassis_t chas, rt_int8_t cmd, void *args)
+{
+    // TODO
+    struct velocity target_vel;
+    
+    LOG_D("received command: cmd:%d arg:%d", cmd, *((rt_uint8_t *)args));
+
+    switch (cmd)
+    {
+    case CHASSIS_FORWARD_ANALOG:
+        target_vel.linear_x = (float)*((rt_uint8_t *)args) / 100.0f;   // m/s
+        target_vel.linear_y = 0;
+        target_vel.angular_z = 0;
+        break;
+    case CHASSIS_BACKWARD_ANALOG:
+        target_vel.linear_x = -(float)*((rt_uint8_t *)args) / 100.0f;
+        target_vel.linear_y = 0;
+        target_vel.angular_z = 0;
+        break;
+    case CHASSIS_TURN_LEFT_ANALOG:
+        target_vel.linear_x = 0.00f;
+        target_vel.linear_y = 0;
+        target_vel.angular_z = (float)*((rt_uint8_t *)args);
+        break;
+    case CHASSIS_TURN_RIGHT_ANALOG:
+        target_vel.linear_x = 0.00f;
+        target_vel.linear_y = 0;
+        target_vel.angular_z = -(float)*((rt_uint8_t *)args);
+        break;
+    case CHASSIS_STOP:
+        target_vel.linear_x = 0.00f;
+        target_vel.linear_y = 0;
+        target_vel.angular_z = 0;
+        break;
+    default:
+        return RT_ERROR;
+        break;
+    }
+
+    chassis_set_velocity(chas, target_vel);
+
+    return RT_EOK;
+}

chassis/chassis.h

@@ -4,6 +4,15 @@
 #include <kinematics.h>
 #include <wheel.h>
 
+enum chassis_command
+{
+    CHASSIS_FORWARD_ANALOG,
+    CHASSIS_BACKWARD_ANALOG,
+    CHASSIS_TURN_LEFT_ANALOG,
+    CHASSIS_TURN_RIGHT_ANALOG,
+    CHASSIS_STOP,
+};
+
 typedef struct chassis *chassis_t;
 
 struct chassis
@@ -25,4 +34,6 @@ rt_err_t    chassis_set_rpm(chassis_t chas, rt_int16_t target_rpm[]);
 
 rt_err_t    chassis_update(chassis_t chas);
 
+rt_err_t    chassis_parse_command(chassis_t chas, rt_int8_t cmd, void *args);
+
 #endif