add set_param interface and implement command-pid

Author: sogwms

chassis/chassis.c

@@ -186,25 +186,90 @@ 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)
     {
         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
     command_register(COMMAND_CAR_STOP, chassis_stop);
 
     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;
 }