add set_param interface and implement command-pid

chassis/chassis.c

@@ -186,6 +186,8 @@ static void chassis_move_right(command_info_t info, void *param, rt_uint16_t siz
     }
 }
 
+// TODO
+
 static void chassis_forward_with_param(command_info_t info, void *param, rt_uint16_t size)
 {
     if (info->target != RT_NULL)
@@ -193,11 +195,70 @@ static void chassis_forward_with_param(command_info_t info, void *param, rt_uint
         if (size == sizeof(struct cmd_dt_velocity))
         {
             struct cmd_dt_velocity *velocity_info = (struct cmd_dt_velocity *)param;
+            if (velocity_info->data.linear_x < 0)
+            {
+                return;
+            }
             chassis_straight(info->target, velocity_info->data.linear_x);
         }
     }
 }
 
+// TODO
+
+static void chassis_set_linear_x(command_info_t info, void *param, rt_uint16_t size)
+{
+    if (info->target != RT_NULL)
+    {
+        if (size == sizeof(struct cmd_dt_velocity))
+        {
+            struct cmd_dt_velocity *velocity_info = (struct cmd_dt_velocity *)param;
+            chassis_straight(info->target, velocity_info->data.linear_x);
+        }
+    }
+}
+
+static void chassis_set_pid(command_info_t info, void *param, rt_uint16_t size)
+{
+    if (info->target != RT_NULL)
+    {
+        if(size == sizeof(struct cmd_dt_pid))
+        {
+            chassis_t chas = (chassis_t)info->target;
+            struct cmd_dt_pid *pid_info = (struct cmd_dt_pid *)param;
+            struct controller_param parameter = {
+                .data.pid.kp = pid_info->kp,
+                .data.pid.ki = pid_info->ki,
+                .data.pid.kd = pid_info->kd
+            };
+            
+            switch (pid_info->id)
+            {
+            case PID_ID_WHEEL_0:
+                controller_set_param(chas->c_wheels[0]->w_controller, &parameter);
+                break;
+            case PID_ID_WHEEL_1:
+                controller_set_param(chas->c_wheels[1]->w_controller, &parameter);
+                break;
+            case PID_ID_WHEEL_2:
+                if (chas->c_kinematics->total_wheels > 2)
+                {
+                    controller_set_param(chas->c_wheels[2]->w_controller, &parameter);
+                }
+                break;
+            case PID_ID_WHEEL_3:
+                if (chas->c_kinematics->total_wheels > 3)
+                {
+                    controller_set_param(chas->c_wheels[3]->w_controller, &parameter);
+                }
+                break;
+            default:
+                break;
+            }
+        }
+    }
+}
+
 static int chassis_command_register(void)
 {
     // TODO
@@ -205,6 +266,10 @@ static int chassis_command_register(void)
     
     command_register(COMMAND_CAR_FORWARD_WITH_PARAM, chassis_forward_with_param);
 
+    command_register(COMMAND_SET_CAR_VELOCITY_LINEAR_X, chassis_set_linear_x);
+
+    command_register(COMMAND_SET_PID, chassis_set_pid);
+
     return RT_EOK;
 }
 

controller/controller.c

@@ -89,3 +89,10 @@ rt_err_t controller_set_sample_time(controller_t controller, rt_uint16_t sample_
     controller->sample_time = sample_time;
     return RT_EOK;
 }
+
+rt_err_t controller_set_param(controller_t controller, controller_param_t param)
+{
+    RT_ASSERT(controller != RT_NULL);
+
+    return controller->set_param(controller, param);
+}

controller/controller.h

@@ -3,16 +3,34 @@
 
 #include <rtthread.h>
 
+struct controller_pid_param
+{
+    float kp;
+    float ki;
+    float kd;
+};
+
+struct controller_param
+{
+    union
+    {
+        struct controller_pid_param pid;
+    } data;
+};
+
+typedef struct controller_param *controller_param_t;
+
 struct controller
 {
-    float       target;
-    float       output;
-    rt_uint16_t sample_time;    // unit:ms
-    int         enable;
-
-    rt_err_t    (*update)(void *controller, float current_point);
-    rt_err_t    (*reset)(void *controller);
-    rt_err_t    (*destroy)(void *controller);
+    float           target;
+    float           output;
+    rt_uint16_t     sample_time;    // unit:ms
+    int             enable;
+
+    rt_err_t        (*update)(void *controller, float current_point);
+    rt_err_t        (*reset)(void *controller);
+    rt_err_t        (*destroy)(void *controller);
+    rt_err_t        (*set_param)(void *controller, controller_param_t param);
 };
 
 typedef struct controller *controller_t;
@@ -25,5 +43,5 @@ rt_err_t        controller_disable(controller_t controller);
 rt_err_t        controller_reset(controller_t controller);
 rt_err_t        controller_set_target(controller_t controller, rt_int16_t target);
 rt_err_t        controller_set_sample_time(controller_t controller, rt_uint16_t sample_time);
-
+rt_err_t        controller_set_param(controller_t controller, controller_param_t param);
 #endif // __CONTROLLER_H__

controller/inc_pid_controller.c

@@ -16,6 +16,17 @@ static rt_err_t inc_pid_controller_destroy(void *pid)
     return RT_EOK;
 }
 
+static rt_err_t inc_pid_controller_set_param(void *pid, controller_param_t param)
+{
+    inc_pid_controller_t inc_pid = (inc_pid_controller_t)pid;
+
+    inc_pid->kp = param->data.pid.kp;
+    inc_pid->ki = param->data.pid.ki;
+    inc_pid->kd = param->data.pid.kd;
+
+    return RT_EOK;
+}
+
 static rt_err_t inc_pid_controller_update(void *pid, float current_point)
 {
     inc_pid_controller_t inc_pid = (inc_pid_controller_t)pid;
@@ -88,6 +99,7 @@ inc_pid_controller_t inc_pid_controller_create(float kp, float ki, float kd, rt_
     new_pid->controller.reset = inc_pid_controller_reset;
     new_pid->controller.destroy = inc_pid_controller_destroy;
     new_pid->controller.update = inc_pid_controller_update;
+    new_pid->controller.set_param = inc_pid_controller_set_param;
 
     return new_pid;
 }

controller/pos_pid_controller.c

@@ -16,6 +16,17 @@ static rt_err_t pos_pid_controller_destroy(void *pid)
     return RT_EOK;
 }
 
+static rt_err_t pos_pid_controller_set_param(void *pid, controller_param_t param)
+{
+    pos_pid_controller_t pos_pid = (pos_pid_controller_t)pid;
+
+    pos_pid->kp = param->data.pid.kp;
+    pos_pid->ki = param->data.pid.ki;
+    pos_pid->kd = param->data.pid.kd;
+
+    return RT_EOK;
+}
+
 static rt_err_t pos_pid_controller_update(void *pid, float current_point)
 {
     pos_pid_controller_t pos_pid = (pos_pid_controller_t)pid;
@@ -97,6 +108,7 @@ pos_pid_controller_t pos_pid_controller_create(float kp, float ki, float kd, rt_
     new_pid->controller.reset = pos_pid_controller_reset;
     new_pid->controller.destroy = pos_pid_controller_destroy;
     new_pid->controller.update = pos_pid_controller_update;
+    new_pid->controller.set_param = pos_pid_controller_set_param;
 
     return new_pid;
 }

protocol/ano.c

@@ -111,17 +111,17 @@ static void ano_parse_frame(uint8_t *buffer, uint8_t length)
         float kpid[9];
         _get_pid_param(buffer, kpid);
         
-        pid.id = 1;
+        pid.id = PID_ID_WHEEL_0;
         pid.kp = kpid[0];
         pid.ki = kpid[1];
         pid.kd = kpid[2];
         command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_dt_pid), ano_target);
-        pid.id = 2;
+        pid.id = PID_ID_WHEEL_1;
         pid.kp = kpid[3];
         pid.ki = kpid[4];
         pid.kd = kpid[5];
         command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_dt_pid), ano_target);
-        pid.id = 3;
+        pid.id = PID_ID_WHEEL_2;
         pid.kp = kpid[6];
         pid.ki = kpid[7];
         pid.kd = kpid[8];
@@ -135,117 +135,117 @@ static void ano_parse_frame(uint8_t *buffer, uint8_t length)
         float kpid[9];
         _get_pid_param(buffer, kpid);
         
-        pid.id = 4;
+        pid.id = PID_ID_WHEEL_3;
         pid.kp = kpid[0];
         pid.ki = kpid[1];
         pid.kd = kpid[2];
         command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_dt_pid), ano_target);
-        pid.id = 5;
-        pid.kp = kpid[3];
-        pid.ki = kpid[4];
-        pid.kd = kpid[5];
-        command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_dt_pid), ano_target);
-        pid.id = 6;
-        pid.kp = kpid[6];
-        pid.ki = kpid[7];
-        pid.kd = kpid[8];
-        command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_dt_pid), ano_target);
+        // pid.id = 5;
+        // pid.kp = kpid[3];
+        // pid.ki = kpid[4];
+        // pid.kd = kpid[5];
+        // command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_dt_pid), ano_target);
+        // pid.id = 6;
+        // pid.kp = kpid[6];
+        // pid.ki = kpid[7];
+        // pid.kd = kpid[8];
+        // command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_dt_pid), ano_target);
 
         ano_send_check(*(buffer + 2), sum);
     }
     else if (*(buffer + 2) == 0X12) //PID3
     {
-        struct cmd_dt_pid pid;
-        float kpid[9];
-        _get_pid_param(buffer, kpid);
+        // struct cmd_dt_pid pid;
+        // float kpid[9];
+        // _get_pid_param(buffer, kpid);
         
-        pid.id = 7;
-        pid.kp = kpid[0];
-        pid.ki = kpid[1];
-        pid.kd = kpid[2];
-        command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_dt_pid), ano_target);
-        pid.id = 8;
-        pid.kp = kpid[3];
-        pid.ki = kpid[4];
-        pid.kd = kpid[5];
-        command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_dt_pid), ano_target);
-        pid.id = 9;
-        pid.kp = kpid[6];
-        pid.ki = kpid[7];
-        pid.kd = kpid[8];
-        command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_dt_pid), ano_target);
+        // pid.id = 7;
+        // pid.kp = kpid[0];
+        // pid.ki = kpid[1];
+        // pid.kd = kpid[2];
+        // command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_dt_pid), ano_target);
+        // pid.id = 8;
+        // pid.kp = kpid[3];
+        // pid.ki = kpid[4];
+        // pid.kd = kpid[5];
+        // command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_dt_pid), ano_target);
+        // pid.id = 9;
+        // pid.kp = kpid[6];
+        // pid.ki = kpid[7];
+        // pid.kd = kpid[8];
+        // command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_dt_pid), ano_target);
 
         ano_send_check(*(buffer + 2), sum);
     }
     else if (*(buffer + 2) == 0X13) //PID4
     {
-        struct cmd_dt_pid pid;
-        float kpid[9];
-        _get_pid_param(buffer, kpid);
+        // struct cmd_dt_pid pid;
+        // float kpid[9];
+        // _get_pid_param(buffer, kpid);
         
-        pid.id = 10;
-        pid.kp = kpid[0];
-        pid.ki = kpid[1];
-        pid.kd = kpid[2];
-        command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_dt_pid), ano_target);
-        pid.id = 11;
-        pid.kp = kpid[3];
-        pid.ki = kpid[4];
-        pid.kd = kpid[5];
-        command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_dt_pid), ano_target);
-        pid.id = 12;
-        pid.kp = kpid[6];
-        pid.ki = kpid[7];
-        pid.kd = kpid[8];
-        command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_dt_pid), ano_target);
+        // pid.id = 10;
+        // pid.kp = kpid[0];
+        // pid.ki = kpid[1];
+        // pid.kd = kpid[2];
+        // command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_dt_pid), ano_target);
+        // pid.id = 11;
+        // pid.kp = kpid[3];
+        // pid.ki = kpid[4];
+        // pid.kd = kpid[5];
+        // command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_dt_pid), ano_target);
+        // pid.id = 12;
+        // pid.kp = kpid[6];
+        // pid.ki = kpid[7];
+        // pid.kd = kpid[8];
+        // command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_dt_pid), ano_target);
 
         ano_send_check(*(buffer + 2), sum);
     }
     else if (*(buffer + 2) == 0X14) //PID5
     {
-        struct cmd_dt_pid pid;
-        float kpid[9];
-        _get_pid_param(buffer, kpid);
+        // struct cmd_dt_pid pid;
+        // float kpid[9];
+        // _get_pid_param(buffer, kpid);
         
-        pid.id = 13;
-        pid.kp = kpid[0];
-        pid.ki = kpid[1];
-        pid.kd = kpid[2];
-        command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_dt_pid), ano_target);
-        pid.id = 14;
-        pid.kp = kpid[3];
-        pid.ki = kpid[4];
-        pid.kd = kpid[5];
-        command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_dt_pid), ano_target);
-        pid.id = 15;
-        pid.kp = kpid[6];
-        pid.ki = kpid[7];
-        pid.kd = kpid[8];
-        command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_dt_pid), ano_target);
+        // pid.id = 13;
+        // pid.kp = kpid[0];
+        // pid.ki = kpid[1];
+        // pid.kd = kpid[2];
+        // command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_dt_pid), ano_target);
+        // pid.id = 14;
+        // pid.kp = kpid[3];
+        // pid.ki = kpid[4];
+        // pid.kd = kpid[5];
+        // command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_dt_pid), ano_target);
+        // pid.id = 15;
+        // pid.kp = kpid[6];
+        // pid.ki = kpid[7];
+        // pid.kd = kpid[8];
+        // command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_dt_pid), ano_target);
 
         ano_send_check(*(buffer + 2), sum);
     }
     else if (*(buffer + 2) == 0X15) //PID6
     {
-        struct cmd_dt_pid pid;
-        float kpid[9];
-        _get_pid_param(buffer, kpid);
+        // struct cmd_dt_pid pid;
+        // float kpid[9];
+        // _get_pid_param(buffer, kpid);
         
-        pid.id = 16;
-        pid.kp = kpid[0];
-        pid.ki = kpid[1];
-        pid.kd = kpid[2];
-        command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_dt_pid), ano_target);
-        pid.id = 17;
-        pid.kp = kpid[3];
-        pid.ki = kpid[4];
-        pid.kd = kpid[5];
-        command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_dt_pid), ano_target);
-        pid.id = 18;
-        pid.kp = kpid[6];
-        pid.ki = kpid[7];
-        pid.kd = kpid[8];
-        command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_dt_pid), ano_target);
+        // pid.id = 16;
+        // pid.kp = kpid[0];
+        // pid.ki = kpid[1];
+        // pid.kd = kpid[2];
+        // command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_dt_pid), ano_target);
+        // pid.id = 17;
+        // pid.kp = kpid[3];
+        // pid.ki = kpid[4];
+        // pid.kd = kpid[5];
+        // command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_dt_pid), ano_target);
+        // pid.id = 18;
+        // pid.kp = kpid[6];
+        // pid.ki = kpid[7];
+        // pid.kd = kpid[8];
+        // command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_dt_pid), ano_target);
 
         ano_send_check(*(buffer + 2), sum);
     }

protocol/command.h

@@ -31,6 +31,12 @@
 #define COMMAND_SEND_SENSOR                     0x6001
 #define COMMAND_SEND_RPY                        0x6002
 
+// PID ID
+#define PID_ID_WHEEL_0                          1
+#define PID_ID_WHEEL_1                          2
+#define PID_ID_WHEEL_2                          3
+#define PID_ID_WHEEL_3                          4
+
 struct cmd_dt_pid
 {
     int   id;       // range: 1 ~ max