pending PID tuning; now car moves in S shape

frtos/config/motor_config.h

@@ -34,7 +34,6 @@
  * @param distance_cm Distance travelled in cm
  */
 typedef struct {
-    float    target_cms;
     float    current_cms;
     float    distance_cm;
 } motor_speed_t;

frtos/motor/motor_direction.h

@@ -19,12 +19,28 @@
 void
 set_wheel_direction(uint32_t direction)
 {
-    static const uint32_t mask = DIRECTION_LEFT_FORWARD |
+    static const uint32_t mask
-                                 DIRECTION_LEFT_BACKWARD |
+        = DIRECTION_LEFT_FORWARD | DIRECTION_LEFT_BACKWARD
-                                 DIRECTION_RIGHT_FORWARD |
+          | DIRECTION_RIGHT_FORWARD | DIRECTION_RIGHT_BACKWARD;
-                                 DIRECTION_RIGHT_BACKWARD;
 
     gpio_put_masked(mask, 0U);
     gpio_set_mask(direction);
 }
 
+/*!
+ * @brief Set the speed of the wheels
+ * @param speed The speed of the wheels, from 0 to 5000
+ */
+void
+set_wheel_speed(uint32_t speed)
+{
+    g_motor_right.pwm.level = speed;
+    g_motor_left.pwm.level  = speed;
+
+    pwm_set_chan_level(g_motor_right.pwm.slice_num,
+                       g_motor_right.pwm.channel,
+                       g_motor_right.pwm.level);
+    pwm_set_chan_level(g_motor_left.pwm.slice_num,
+                       g_motor_left.pwm.channel,
+                       g_motor_left.pwm.level);
+}

frtos/motor/motor_init.h

@@ -19,14 +19,6 @@
 
 #include "motor_config.h"
 
-// TODO: tune pid for both wheels again
-/*
- * ultimate gain Ku about 14, ultimate period Tu about 8 * 50 = 400ms
- * Ku = 14, Tu = 400ms,
- * Kp = 0.6 * Ku = 8.4
- * Ki = Kp / Tu = 0.021
- * Kd = Kp * Tu / 8 = 42
- */
 motor_t g_motor_left  = { .pwm.level         = 0u,
                           .pwm.channel       = PWM_CHAN_A,
                           .speed.distance_cm = 0.0f,
@@ -37,9 +29,9 @@ motor_t g_motor_left  = { .pwm.level         = 0u,
 motor_t g_motor_right = { .pwm.level         = 0u,
                           .pwm.channel       = PWM_CHAN_B,
                           .speed.distance_cm = 0.0f,
-                          .pid.kp_value      = 0.0f,
+                          .pid.kp_value      = 8.4f,
-                          .pid.ki_value      = 0.0f,
+                          .pid.ki_value      = 0.021f,
-                          .pid.kd_value      = 0.0f,};
+                          .pid.kd_value      = 42.0f,};
 
 void
 motor_init(void)

frtos/motor/motor_pid.h

@@ -16,20 +16,20 @@
  * @return The control signal
  */
 float
-compute_pid(const volatile motor_t *p_motor,
+compute_pid(float *integral, float *prev_error)
-            float                *integral,
-            float                *prev_error)
 {
-    float error = p_motor->speed.target_cms - p_motor->speed.current_cms;
+    float error
+        = g_motor_left.speed.distance_cm - g_motor_right.speed.distance_cm;
+
+    printf("error: %f\n", error);
 
     *integral += error;
 
     float derivative = error - *prev_error;
 
-    float control_signal
+    float control_signal = g_motor_right.pid.kp_value * error
-        = p_motor->pid.kp_value * error +
+                           + g_motor_right.pid.ki_value * (*integral)
-          p_motor->pid.ki_value * (*integral) +
+                           + g_motor_right.pid.kd_value * derivative;
-          p_motor->pid.kd_value * derivative;
 
     *prev_error = error;
 
@@ -37,45 +37,42 @@ compute_pid(const volatile motor_t *p_motor,
 }
 
 void
-motor_pid_task(void *p_param)
+motor_pid_task(__unused void *p_param)
 {
-    motor_t *p_motor   = p_param;
     float integral   = 0.0f;
     float prev_error = 0.0f;
 
     for (;;)
     {
-        if (p_motor->speed.target_cms == 0.0f)
+        if (g_motor_left.pwm.level == 0u)
         {
-            p_motor->pwm.level = 0;
+            g_motor_right.pwm.level = 0;
-            pwm_set_chan_level(p_motor->pwm.slice_num,
+            pwm_set_chan_level(g_motor_right.pwm.slice_num,
-                               p_motor->pwm.channel,
+                               g_motor_right.pwm.channel,
-                               p_motor->pwm.level);
+                               g_motor_right.pwm.level);
             vTaskDelay(pdMS_TO_TICKS(50));
             continue;
         }
 
-        float control_signal = compute_pid(p_motor, &integral, &prev_error);
+        float control_signal = compute_pid(&integral, &prev_error);
 
-        if (p_motor->pwm.level + control_signal > MAX_SPEED)
+        float temp = (float) g_motor_right.pwm.level + control_signal;
+
+        if (temp > MAX_SPEED)
         {
-            p_motor->pwm.level = MAX_SPEED;
+            temp = MAX_SPEED;
-        }
-        else if (p_motor->pwm.level + control_signal < MIN_SPEED)
-        {
-            p_motor->pwm.level = MIN_SPEED;
-        }
-        else
-        {
-            p_motor->pwm.level = p_motor->pwm.level + control_signal;
         }
 
-        // printf("control signal: %f\n", control_signal);
+        if (temp < MIN_SPEED)
-        // printf("new pwm: %hu\n\n", p_motor_speed->level);
+        {
+            temp = MIN_SPEED;
+        }
 
-        pwm_set_chan_level(p_motor->pwm.slice_num,
+        g_motor_right.pwm.level = (uint16_t) temp;
-                           p_motor->pwm.channel,
+
-                           p_motor->pwm.level);
+        pwm_set_chan_level(g_motor_right.pwm.slice_num,
+                           g_motor_right.pwm.channel,
+                           g_motor_right.pwm.level);
 
         vTaskDelay(pdMS_TO_TICKS(50));
     }

frtos/motor/motor_speed.h

@@ -63,18 +63,13 @@ monitor_wheel_speed_task(void *pvParameters)
             // circumference = 2 * pi * 3.25 cm = 20.4203522483 cm
             // distance = 20.4203522483 cm / 20 = 1.02101761242 cm
             p_motor->speed.current_cms
-                = (float) (1.02101761242f / (elapsed_time / 1000000.f));
+                = (float) (1021017.61242f / elapsed_time);
 
             p_motor->speed.distance_cm += 1.02101761242f;
-
-            // printf("speed: %f cm/s\n", p_motor_speed->current_cms);
         }
         else
         {
             p_motor->speed.current_cms = 0.f;
-            // printf("stopped\n");
+        }
-        }
-
-        // printf("distance: %f cm\n", p_motor_speed->distance);
     }
 }

frtos/motor/motor_test.c

@@ -6,42 +6,6 @@
 
 #define WHEEL_SPEED_PRIO (tskIDLE_PRIORITY + 1UL)
 
-void
-test_speed_change_task(void *p_param)
-{
-    for (;;)
-    {
-        g_motor_left.speed.target_cms  = 30.0f;
-        g_motor_right.speed.target_cms = 30.0f;
-        vTaskDelay(pdMS_TO_TICKS(5000));
-
-        g_motor_left.speed.target_cms  = 20.0f;
-        g_motor_right.speed.target_cms = 20.0f;
-        vTaskDelay(pdMS_TO_TICKS(5000));
-
-        g_motor_left.speed.target_cms  = 0.0f;
-        g_motor_right.speed.target_cms = 0.0f;
-        vTaskDelay(pdMS_TO_TICKS(5000));
-
-        set_wheel_direction(DIRECTION_LEFT_BACKWARD | DIRECTION_RIGHT_BACKWARD);
-
-        g_motor_left.speed.target_cms  = 30.0f;
-        g_motor_right.speed.target_cms = 30.0f;
-        vTaskDelay(pdMS_TO_TICKS(5000));
-
-        g_motor_left.speed.target_cms  = 20.0f;
-        g_motor_right.speed.target_cms = 20.0f;
-        vTaskDelay(pdMS_TO_TICKS(5000));
-
-        g_motor_left.speed.target_cms  = 0.0f;
-        g_motor_right.speed.target_cms = 0.0f;
-        vTaskDelay(pdMS_TO_TICKS(5000));
-
-        set_wheel_direction(DIRECTION_LEFT_FORWARD | DIRECTION_RIGHT_FORWARD);
-
-    }
-}
-
 void
 launch()
 {
@@ -55,6 +19,9 @@ launch()
 
     irq_set_enabled(IO_IRQ_BANK0, true);
 
+    // Set wheel speed
+    set_wheel_speed(3000);
+
     // Left wheel
     //
     TaskHandle_t h_monitor_left_wheel_speed_task_handle = NULL;
@@ -65,13 +32,6 @@ 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_left,
-                WHEEL_SPEED_PRIO,
-                &h_motor_pid_left_task_handle);
 
     // Right wheel
     //
@@ -91,15 +51,6 @@ launch()
                 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",
-                configMINIMAL_STACK_SIZE,
-                NULL,
-                WHEEL_SPEED_PRIO,
-                &h_test_speed_change_task_handle);
 
     vTaskStartScheduler();
 }