use single slice number; change PID to match left pwm with right distance travelled.

frtos/config/motor_config.h

@@ -6,6 +6,8 @@
 #define PWM_PIN_RIGHT 1U // chanel B
 #define PWM_PIN_LEFT  0U // chanel A
 
+#define SLICE_NUM     0U
+
 // IN1, IN2, IN3, IN4 on the L298N
 #define DIRECTION_PIN_RIGHT_IN1 11U
 #define DIRECTION_PIN_RIGHT_IN2 12U
@@ -65,7 +67,6 @@ typedef struct
  */
 typedef struct
 {
-    uint     slice_num;
     uint     channel;
     uint16_t level;
 } motor_pwm_t;

frtos/motor/motor_direction.h

@@ -49,7 +49,8 @@ revert_wheel_direction()
  * @param range acceptable range
  * @return true if the current direction is within the range of the target
  */
-bool check_direction(float current_direction, float target_direction, float range)
+bool
+check_direction(float current_direction, float target_direction, float range)
 {
     // Normalize directions to be within 0 to 360 degrees
     current_direction = fmod(current_direction, 360.0f);
@@ -67,6 +68,34 @@ bool check_direction(float current_direction, float target_direction, float rang
     return false;
 }
 
+void
+turn_by_distance(uint32_t      direction,
+                 float         distance,
+                 uint32_t      pwm_level,
+                 car_struct_t *pp_car_struct)
+{
+    pp_car_struct->p_pid->use_pid = false;
+
+    set_wheel_direction(direction);
+
+    distance_to_stop(pp_car_struct, distance);
+
+    set_wheel_direction(DIRECTION_MASK);
+    set_wheel_speed_synced(0u, pp_car_struct);
+}
+
+/*!
+ * @brief Diameter 12.5cm; circumference 39.269908169872416cm,
+ * 10 degree = circumference / 36 = 1.090831882496456cm
+ */
+void
+turn_by_10_degree(uint32_t      direction,
+                  uint32_t      pwm_level,
+                  car_struct_t *pp_car_struct)
+{
+    turn_by_distance(direction, 1.090831882496456f, pwm_level, pp_car_struct);
+}
+
 /*!
  * @brief Spin the car to a certain yaw specifically
  * @param direction The direction to turn or spin

frtos/motor/motor_init.h

@@ -76,24 +76,16 @@ motor_init(car_struct_t *car_struct)
     gpio_set_function(PWM_PIN_LEFT, GPIO_FUNC_PWM);
     gpio_set_function(PWM_PIN_RIGHT, GPIO_FUNC_PWM);
 
-    car_struct->p_left_motor->pwm.slice_num
-        = pwm_gpio_to_slice_num(PWM_PIN_LEFT);
-    car_struct->p_right_motor->pwm.slice_num
-        = pwm_gpio_to_slice_num(PWM_PIN_RIGHT);
-
     // NOTE: PWM clock is 125MHz for raspberrypi pico w by default
 
     // 125MHz / 50 = 2500kHz
-    pwm_set_clkdiv(car_struct->p_left_motor->pwm.slice_num, PWM_CLK_DIV);
+    pwm_set_clkdiv(SLICE_NUM, PWM_CLK_DIV);
-    pwm_set_clkdiv(car_struct->p_right_motor->pwm.slice_num, PWM_CLK_DIV);
 
     // L289N can accept up to 40kHz
     // 2500kHz / 100 = 25kHz
-    pwm_set_wrap(car_struct->p_left_motor->pwm.slice_num, (PWM_WRAP - 1U));
+    pwm_set_wrap(SLICE_NUM, (PWM_WRAP - 1U));
-    pwm_set_wrap(car_struct->p_right_motor->pwm.slice_num, (PWM_WRAP - 1U));
 
-    pwm_set_enabled(car_struct->p_left_motor->pwm.slice_num, true);
+    pwm_set_enabled(SLICE_NUM, true);
-    pwm_set_enabled(car_struct->p_right_motor->pwm.slice_num, true);
 }
 
 /*!

frtos/motor/motor_pid.h

@@ -18,25 +18,26 @@
 float
 compute_pid(float *integral, float *prev_error, car_struct_t *car_struct)
 {
-    float error = car_struct->p_left_motor->speed.distance_cm
+    //    float error = car_struct->p_left_motor->speed.distance_cm
-                  - car_struct->p_right_motor->speed.distance_cm;
+    //                  - car_struct->p_right_motor->speed.distance_cm;
+
+    float error = car_struct->p_right_motor->speed.distance_cm
+                  - car_struct->p_left_motor->speed.distance_cm;
 
     printf("error: %f\n", error);
     *integral += error;
 
     float derivative = error - *prev_error;
 
-    float control_signal
+    float control_signal = car_struct->p_pid->kp_value * error
-        = car_struct->p_pid->kp_value * error
+                           + car_struct->p_pid->ki_value * (*integral)
-          + car_struct->p_pid->ki_value * (*integral)
+                           + car_struct->p_pid->kd_value * derivative;
-          + car_struct->p_pid->kd_value * derivative;
 
     *prev_error = error;
 
     return control_signal;
 }
 
-
 /*!
  * @brief Repeating timer handler for the PID controller
  * @param ppp_timer The repeating timer
@@ -57,8 +58,12 @@ repeating_pid_handler(struct repeating_timer *ppp_timer)
 
     float control_signal = compute_pid(&integral, &prev_error, car_strut);
 
+    //    float temp
+    //        = (float)car_strut->p_right_motor->pwm.level + control_signal *
+    //        0.05f;
+
     float temp
-        = (float)car_strut->p_right_motor->pwm.level + control_signal * 0.05f;
+        = (float)car_strut->p_left_motor->pwm.level + control_signal * 0.05f;
 
     if (temp > MAX_PWM_LEVEL)
     {
@@ -70,8 +75,8 @@ repeating_pid_handler(struct repeating_timer *ppp_timer)
         temp = MIN_PWM_LEVEL;
     }
 
-    set_wheel_speed((uint32_t)temp, car_strut->p_right_motor);
+    //    set_wheel_speed((uint32_t)temp, car_strut->p_right_motor);
-
+    set_wheel_speed((uint32_t)temp, car_strut->p_left_motor);
     return true;
 }
 

frtos/motor/motor_speed.h

@@ -91,7 +91,7 @@ set_wheel_speed(uint32_t pwm_level, motor_t *p_motor)
     p_motor->pwm.level = pwm_level;
 
     pwm_set_chan_level(
-        p_motor->pwm.slice_num, p_motor->pwm.channel, p_motor->pwm.level);
+        SLICE_NUM, p_motor->pwm.channel, p_motor->pwm.level);
 }
 
 /*!
@@ -102,7 +102,7 @@ set_wheel_speed(uint32_t pwm_level, motor_t *p_motor)
 void
 set_wheel_speed_synced(uint32_t pwm_level, car_struct_t *pp_car_strut)
 {
-    set_wheel_speed(pwm_level, pp_car_strut->p_left_motor);
+    set_wheel_speed(MAX_PWM_LEVEL, pp_car_strut->p_left_motor);
     set_wheel_speed(pwm_level, pp_car_strut->p_right_motor);
 }
 

frtos/motor/motor_test.c

@@ -7,15 +7,8 @@ motor_control_task(void *params)
     car_struct_t *car_struct = (car_struct_t *)params;
     for (;;)
     {
-        set_wheel_direction(DIRECTION_FORWARD);
+        turn_by_10_degree(RIGHT, 90u, car_struct);
-        set_wheel_speed_synced(90u, car_struct);
+        vTaskDelay(pdMS_TO_TICKS(1000));
-
-        vTaskDelay(pdMS_TO_TICKS(10000));
-
-        revert_wheel_direction();
-        set_wheel_speed_synced(90u, car_struct);
-
-        vTaskDelay(pdMS_TO_TICKS(10000));
     }
 }