separate speed control and monitor

frtos/config/motor_config.h

@@ -25,8 +25,8 @@
 #define PWM_WRAP                    5000U
 
 #define PID_KP                      3.f
-#define PID_KI                      0.01f
+#define PID_KI                      0.01 // 0.01f
-#define PID_KD                      0.05f
+#define PID_KD                      0.05f // 0.05f
 
 #define MAX_SPEED                   4900U
 #define MIN_SPEED                   0U    // To be changed
@@ -41,6 +41,7 @@
  */
 typedef struct {
     float               target_speed_cms;
+    float               current_speed_cms;
     uint16_t            pwm_level;
     SemaphoreHandle_t * p_sem;
     uint              * p_slice_num;

frtos/motor/motor_pid.h

@@ -0,0 +1,72 @@
+/*
+ * @file motor_pid.h
+ * @brief control the speed of the wheels by setting the PWM level, using PID
+ * controller
+ * @author Richie
+ */
+
+#include "motor_init.h"
+
+/*!
+ * @brief Compute the control signal using PID controller
+ * @param target_speed The target speed of the wheel
+ * @param current_speed The current speed of the wheel
+ * @param integral The integral term of the PID controller
+ * @param prev_error The previous error of the PID controller
+ * @return The control signal
+ */
+float
+compute_pid(const volatile float *target_speed,
+            const volatile float *current_speed,
+            float                *integral,
+            float                *prev_error)
+{
+    float error = *target_speed - *current_speed;
+    *integral += error;
+
+    float derivative = error - *prev_error;
+
+    float control_signal
+        = PID_KP * error + PID_KI * (*integral) + PID_KD * derivative;
+
+    *prev_error = error;
+
+    return control_signal;
+}
+
+void
+motor_pid_task(void *p_param)
+{
+    motor_speed_t *p_motor_speed = p_param;
+    float         integral       = 0.0f;
+    float         prev_error     = 0.0f;
+
+    for (;;)
+    {
+        float control_signal = compute_pid(&(p_motor_speed->target_speed_cms),
+                                           &(p_motor_speed->current_speed_cms),
+                                           &integral, &prev_error);
+
+        if (p_motor_speed->pwm_level + control_signal > MAX_SPEED)
+        {
+            p_motor_speed->pwm_level = MAX_SPEED;
+        }
+        else if (p_motor_speed->pwm_level + control_signal < MIN_SPEED)
+        {
+            p_motor_speed->pwm_level = MIN_SPEED;
+        }
+        else
+        {
+            p_motor_speed->pwm_level = p_motor_speed->pwm_level + control_signal;
+        }
+
+        printf("control signal: %f\n", control_signal);
+        printf("new pwm: %hu\n\n", p_motor_speed->pwm_level);
+
+        pwm_set_chan_level(*p_motor_speed->p_slice_num,
+                           p_motor_speed->pwm_channel,
+                           p_motor_speed->pwm_level);
+
+        vTaskDelay(pdMS_TO_TICKS(50));
+    }
+}

frtos/motor/motor_speed.h

@@ -1,9 +1,6 @@
 /*
  * @file motor_speed.c
- * @brief control the speed of the wheels by setting the PWM level, and
+ * @brief monitor and update the speed of the wheels
- * monitor the speed by using edge interrupt and measure the time between
- * each slot of the wheel, then calculate the speed of the wheel in cm/s, and
- * adjust the speed of the wheel by using PID controller, and set the new PWM
  * @author Richie
  */
 
@@ -36,32 +33,6 @@ h_wheel_sensor_isr_handler(void)
     }
 }
 
-/*!
- * @brief Compute the control signal using PID controller
- * @param target_speed The target speed of the wheel
- * @param current_speed The current speed of the wheel
- * @param integral The integral term of the PID controller
- * @param prev_error The previous error of the PID controller
- * @return The control signal
- */
-float
-compute_pid(const volatile float *target_speed,
-            const float          *current_speed,
-            float                *integral,
-            float                *prev_error)
-{
-    float error = *target_speed - *current_speed;
-    *integral += error;
-
-    float derivative = error - *prev_error;
-
-    float control_signal
-        = PID_KP * error + PID_KI * (*integral) + PID_KD * derivative;
-
-    *prev_error = error;
-
-    return control_signal;
-}
 
 /*!
  * @brief Task to monitor and control the speed of the wheel
@@ -74,18 +45,10 @@ monitor_wheel_speed_task(void *pvParameters)
     volatile motor_speed_t *p_motor_speed = NULL;
     p_motor_speed                         = (motor_speed_t *)pvParameters;
 
-    float speed = 0.f;
-
-    float new_pwm = p_motor_speed->pwm_level;
-
     uint64_t curr_time    = 0u;
     uint64_t prev_time    = 0u;
     uint64_t elapsed_time = 0u;
 
-    float control_signal = 0.f;
-    float integral       = 0.f;
-    float prev_error     = 0.f;
-
     for (;;)
     {
         if (xSemaphoreTake(*p_motor_speed->p_sem, pdMS_TO_TICKS(100))
@@ -99,37 +62,16 @@ monitor_wheel_speed_task(void *pvParameters)
             // distance = circumference / 20
             // circumference = 2 * pi * 3.25 cm = 20.4203522483 cm
             // distance = 20.4203522483 cm / 20 = 1.02101761242 cm
-            speed = (float)(1.02101761242f / (elapsed_time / 1000000.f));
+            p_motor_speed->current_speed_cms = (float) (1.02101761242f /
+                                                       (elapsed_time /
+                                                        1000000.f));
 
-            printf("speed: %f cm/s\n", speed);
+            printf("speed: %f cm/s\n", p_motor_speed->current_speed_cms);
         }
         else
         {
-            speed = 0.f;
+            p_motor_speed->current_speed_cms = 0.f;
             printf("stopped\n");
         }
-
-        control_signal = compute_pid(
-            &(p_motor_speed->target_speed_cms), &speed, &integral, &prev_error);
-
-        if (new_pwm + control_signal > MAX_SPEED)
-        {
-            new_pwm = MAX_SPEED;
-        }
-        else if (new_pwm + control_signal < MIN_SPEED)
-        {
-            new_pwm = MIN_SPEED;
-        }
-        else
-        {
-            new_pwm = new_pwm + control_signal;
-        }
-
-        printf("control signal: %f\n", control_signal);
-        printf("new pwm: %f\n\n", new_pwm);
-
-        pwm_set_chan_level(*p_motor_speed->p_slice_num,
-                           p_motor_speed->pwm_channel,
-                           (int16_t)new_pwm);
     }
 }

frtos/motor/motor_test.c

@@ -2,6 +2,7 @@
 
 #include "motor_speed.h"
 #include "motor_direction.h"
+#include "motor_pid.h"
 
 #define WHEEL_SPEED_PRIO (tskIDLE_PRIORITY + 1UL)
 
@@ -10,13 +11,18 @@ test_speed_change_task(void *p_param)
 {
     for (;;)
     {
+        g_motor_speed_left.target_speed_cms  = 50.0f;
+        g_motor_speed_right.target_speed_cms = 50.0f;
+        vTaskDelay(pdMS_TO_TICKS(5000));
+
         g_motor_speed_left.target_speed_cms  = 20.0f;
         g_motor_speed_right.target_speed_cms = 20.0f;
-        vTaskDelay(pdMS_TO_TICKS(10000));
+        vTaskDelay(pdMS_TO_TICKS(5000));
+
+        g_motor_speed_left.target_speed_cms  = 0.0f;
+        g_motor_speed_right.target_speed_cms = 0.0f;
+        vTaskDelay(pdMS_TO_TICKS(5000));
 
-        g_motor_speed_left.target_speed_cms  = 40.0f;
-        g_motor_speed_right.target_speed_cms = 40.0f;
-        vTaskDelay(pdMS_TO_TICKS(10000));
     }
 }
 
@@ -33,6 +39,8 @@ launch()
 
     irq_set_enabled(IO_IRQ_BANK0, true);
 
+    // Left wheel
+    //
 //    TaskHandle_t h_monitor_left_wheel_speed_task_handle = NULL;
 //    xTaskCreate(monitor_wheel_speed_task,
 //                "monitor_left_wheel_speed_task",
@@ -41,6 +49,16 @@ launch()
 //                WHEEL_SPEED_PRIO,
 //                &h_monitor_left_wheel_speed_task_handle);
 
+//    TaskHandle_t h_motor_pid_left_task_handle = NULL;
+//    xTaskCreate(motor_pid_task,
+//                "motor_pid_task",
+//                configMINIMAL_STACK_SIZE,
+//                (void *)&g_motor_speed_left,
+//                WHEEL_SPEED_PRIO,
+//                &h_motor_pid_left_task_handle);
+
+    // Right wheel
+    //
     TaskHandle_t h_monitor_right_wheel_speed_task_handle = NULL;
     xTaskCreate(monitor_wheel_speed_task,
                 "monitor_wheel_speed_task",
@@ -49,6 +67,16 @@ launch()
                 WHEEL_SPEED_PRIO,
                 &h_monitor_right_wheel_speed_task_handle);
 
+    TaskHandle_t h_motor_pid_right_task_handle = NULL;
+    xTaskCreate(motor_pid_task,
+                "motor_pid_task",
+                configMINIMAL_STACK_SIZE,
+                (void *)&g_motor_speed_right,
+                WHEEL_SPEED_PRIO,
+                &h_motor_pid_right_task_handle);
+
+    // Test speed change
+    //
     TaskHandle_t h_test_speed_change_task_handle = NULL;
     xTaskCreate(test_speed_change_task,
                 "test_speed_change_task",