Merge branch 'L4ncelot-R:main' into main

frtos/config/motor_config.h

@@ -23,14 +23,16 @@
 #define DIRECTION_LEFT     (DIRECTION_RIGHT_FORWARD | DIRECTION_LEFT_BACKWARD)
 #define DIRECTION_RIGHT    (DIRECTION_RIGHT_BACKWARD | DIRECTION_LEFT_FORWARD)
 
+#define DIRECTION_MASK (DIRECTION_FORWARD | DIRECTION_BACKWARD)
+
 #define SPEED_PIN_RIGHT 15U
 #define SPEED_PIN_LEFT  16U
 
-#define PWM_CLK_DIV 250.f
+#define PWM_CLK_DIV 50.f
-#define PWM_WRAP    5000U
+#define PWM_WRAP    100U
 
-#define MAX_SPEED 4900U
+#define MAX_PWM_LEVEL 99U
-#define MIN_SPEED 0U // To be changed
+#define MIN_PWM_LEVEL 0U
 
 #define WHEEL_SPEED_PRIO    (tskIDLE_PRIORITY + 1UL)
 #define WHEEL_CONTROL_PRIO  (tskIDLE_PRIORITY + 1UL)
@@ -95,4 +97,6 @@ typedef struct
     volatile bool is_running;
 } distance_to_stop_t;
 
+volatile bool g_use_pid = true;
+
 #endif /* MOTOR_CONFIG_H */

frtos/motor/motor_direction.h

@@ -5,6 +5,7 @@
  */
 
 #include "motor_init.h"
+#include "magnetometer_init.h"
 
 /*!
  * @brief Set the direction of the wheels; can use bitwise OR to set both
@@ -19,19 +20,29 @@
 void
 set_wheel_direction(uint32_t direction)
 {
-    static const uint32_t mask
+    gpio_put_masked(DIRECTION_MASK, 0U);
-        = DIRECTION_LEFT_FORWARD | DIRECTION_LEFT_BACKWARD
-          | DIRECTION_RIGHT_FORWARD | DIRECTION_RIGHT_BACKWARD;
-
-    gpio_put_masked(mask, 0U);
     gpio_set_mask(direction);
 }
 
+/*!
+ * @brief Set the direction of the wheel to opposite direction using bit mask
+ */
+void
+revert_wheel_direction()
+{
+    uint32_t current_direction = gpio_get_all();
+
+    uint32_t reverted_direction = current_direction ^ DIRECTION_MASK;
+
+    gpio_put_masked(DIRECTION_MASK, 0U);
+    gpio_set_mask(reverted_direction & DIRECTION_MASK);
+}
+
 void
 turn_left_90()
 {
     set_wheel_direction(DIRECTION_RIGHT_FORWARD);
-    set_wheel_speed(3500u, &g_motor_right);
+    set_wheel_speed(90u, &g_motor_right);
 
     float initial = g_motor_right.speed.distance_cm;
     for (;;)
@@ -52,7 +63,7 @@ void
 turn_right_90()
 {
     set_wheel_direction(DIRECTION_LEFT_FORWARD);
-    set_wheel_speed(3500u, &g_motor_left);
+    set_wheel_speed(90u, &g_motor_left);
 
     float initial = g_motor_left.speed.distance_cm;
     for (;;)
@@ -74,7 +85,7 @@ spin_left_90()
 {
     set_wheel_direction(DIRECTION_LEFT);
 
-    set_wheel_speed_synced(3500u);
+    set_wheel_speed_synced(90u);
 
     float initial = g_motor_left.speed.distance_cm;
     for (;;)
@@ -96,7 +107,7 @@ spin_right_90()
 {
     set_wheel_direction(DIRECTION_RIGHT);
 
-    set_wheel_speed_synced(3500u);
+    set_wheel_speed_synced(90u);
 
     float initial = g_motor_right.speed.distance_cm;
     for (;;)
@@ -111,4 +122,38 @@ spin_right_90()
     vTaskDelay(pdMS_TO_TICKS(1000));
     g_motor_right.speed.distance_cm = initial;
     g_motor_left.speed.distance_cm  = initial;
+}
+
+void
+spin_to_yaw(float target_yaw)
+{
+    float initial_yaw = g_direction.yaw;
+
+    // if it will to turn more than 180 degrees, turn the other way
+    if ((target_yaw > initial_yaw) && (target_yaw - initial_yaw < 180.f)
+        || ((target_yaw < initial_yaw) && (initial_yaw - target_yaw >= 180.f)))
+    {
+        set_wheel_direction(DIRECTION_LEFT);
+    }
+    else if ((target_yaw > initial_yaw) && (target_yaw - initial_yaw >= 180.f)
+             || ((target_yaw < initial_yaw)
+                 && (initial_yaw - target_yaw < 180.f)))
+    {
+        set_wheel_direction(DIRECTION_RIGHT);
+    }
+
+    set_wheel_speed_synced(90u);
+
+    g_use_pid = false;
+
+    for (;;)
+    {
+        if (initial_yaw == target_yaw)
+        {
+            set_wheel_speed_synced(0u);
+            break;
+        }
+        vTaskDelay(pdMS_TO_TICKS(5));
+    }
+    g_use_pid = true;
 }

frtos/motor/motor_init.h

@@ -23,12 +23,17 @@ motor_t g_motor_left  = { .pwm.level         = 0u,
                           .pwm.channel       = PWM_CHAN_A,
                           .speed.distance_cm = 0.0f };
 
+// classic ziegler nichols method
+// Ku = 1000, Tu = 0.9s, interval = 0.05s
+// Kp = 0.6 * Ku = 600
+// Ki = 2 * Kp * 0.05 / Tu = 66.67
+// Kd = Kp * Tu * 0.05 / 8 = 1350
 motor_t g_motor_right = { .pwm.level         = 0u,
                           .pwm.channel       = PWM_CHAN_B,
                           .speed.distance_cm = 0.0f,
-                          .pid.kp_value      = 1000.f,
+                          .pid.kp_value      = 600.f,
-                          .pid.ki_value      = 0.0f,
+                          .pid.ki_value      = 66.67f,
-                          .pid.kd_value      = 10000.0f,};
+                          .pid.kd_value      = 1350.f,};
 
 void
 motor_init(void)
@@ -62,11 +67,12 @@ motor_init(void)
 
     // NOTE: PWM clock is 125MHz for raspberrypi pico w by default
 
-    // 125MHz / 250 = 500kHz
+    // 125MHz / 50 = 2500kHz
     pwm_set_clkdiv(g_motor_left.pwm.slice_num, PWM_CLK_DIV);
     pwm_set_clkdiv(g_motor_right.pwm.slice_num, PWM_CLK_DIV);
 
-    // have them to be 500kHz / 5000 = 100Hz
+    // L289N can accept up to 40kHz
+    // 2500kHz / 100 = 25kHz
     pwm_set_wrap(g_motor_left.pwm.slice_num, (PWM_WRAP - 1U));
     pwm_set_wrap(g_motor_right.pwm.slice_num, (PWM_WRAP - 1U));
 

frtos/motor/motor_pid.h

@@ -21,7 +21,7 @@ compute_pid(float *integral, float *prev_error)
     float error
         = g_motor_left.speed.distance_cm - g_motor_right.speed.distance_cm;
 
-    printf("%f,\n", error);
+    printf("%f\n", error);
 
     *integral += error;
 
@@ -42,7 +42,7 @@ repeating_pid_handler(__unused struct repeating_timer *t)
     static float integral   = 0.0f;
     static float prev_error = 0.0f;
 
-    if ((g_motor_left.pwm.level == 0u) || (g_motor_right.pwm.level == 0u))
+    if (!g_use_pid)
     {
         return true;
     }
@@ -51,14 +51,14 @@ repeating_pid_handler(__unused struct repeating_timer *t)
 
     float temp = (float)g_motor_right.pwm.level + control_signal * 0.05f;
 
-    if (temp > MAX_SPEED)
+    if (temp > MAX_PWM_LEVEL)
     {
-        temp = MAX_SPEED;
+        temp = MAX_PWM_LEVEL;
     }
 
-    if (temp < MIN_SPEED)
+    if (temp <= MIN_PWM_LEVEL)
     {
-        temp = MIN_SPEED + 1u;
+        temp = MIN_PWM_LEVEL + 1u;
     }
 
     g_motor_right.pwm.level = (uint16_t)temp;
@@ -66,8 +66,8 @@ repeating_pid_handler(__unused struct repeating_timer *t)
                        g_motor_right.pwm.channel,
                        g_motor_right.pwm.level);
 
-    printf("speed: %f cm/s\n", g_motor_right.speed.current_cms);
+//    printf("speed: %f cm/s\n", g_motor_right.speed.current_cms);
-    printf("distance: %f cm\n", g_motor_right.speed.distance_cm);
+//    printf("distance: %f cm\n", g_motor_right.speed.distance_cm);
 
     return true;
 }

frtos/motor/motor_speed.h

@@ -51,8 +51,8 @@ monitor_wheel_speed_task(void *pvParameters)
 
     for (;;)
     {
-        if (xSemaphoreTake(p_motor->sem, pdMS_TO_TICKS(100))
+        if ((xSemaphoreTake(p_motor->sem, pdMS_TO_TICKS(100))
-            == pdTRUE)
+            == pdTRUE) && (g_use_pid == true))
         {
             curr_time    = time_us_64();
             elapsed_time = curr_time - prev_time;
@@ -86,11 +86,16 @@ set_wheel_speed(uint32_t pwm_level, motor_t * motor)
 
 /*!
  * @brief Set the speed of the wheels
- * @param pwm_level The pwm_level of the wheels, from 0 to 5000
+ * @param pwm_level The pwm_level of the wheels, from 0 to 99
  */
 void
 set_wheel_speed_synced(uint32_t pwm_level)
 {
+    if (pwm_level > MAX_PWM_LEVEL)
+    {
+        pwm_level = MAX_PWM_LEVEL;
+    }
+
     set_wheel_speed(pwm_level, &g_motor_left);
     set_wheel_speed(pwm_level, &g_motor_right);
 }

frtos/motor/motor_test.c

@@ -4,6 +4,22 @@
 #include "motor_direction.h"
 #include "motor_pid.h"
 
+void
+motor_control_task(__unused void *params)
+{
+    for (;;)
+    {
+        set_wheel_direction(DIRECTION_FORWARD);
+        set_wheel_speed_synced(90);
+
+        vTaskDelay(pdMS_TO_TICKS(10000));
+
+        revert_wheel_direction();
+        set_wheel_speed_synced(90);
+
+        vTaskDelay(pdMS_TO_TICKS(10000));
+    }
+}
 
 void
 launch()