updated vtask and init

frtos/config/motor_config.h

@@ -87,8 +87,9 @@ typedef struct
 {
     motor_speed_t       speed;
     motor_pwm_t         pwm;
-    SemaphoreHandle_t *p_sem;
+    SemaphoreHandle_t * p_sem;
-    motor_pid_t       *p_pid;
+    bool              * use_pid;
+
 } motor_t;
 
 typedef struct
@@ -103,8 +104,9 @@ SemaphoreHandle_t g_right_sem;
 
 typedef struct
 {
-    motor_t     *p_left_motor;
+    motor_t     * p_left_motor;
-    motor_t     *p_right_motor;
+    motor_t     * p_right_motor;
+    motor_pid_t * p_pid;
 
 } car_struct_t;
 

frtos/motor/motor_direction.h

@@ -85,7 +85,7 @@ spin_to_yaw(float target_yaw, car_struct_t *car_struct)
 
     set_wheel_speed_synced(80u, car_struct);
 
-    car_struct->p_right_motor->p_pid->use_pid = false;
+    car_struct->p_pid->use_pid = false;
 
     for (;;)
     {
@@ -98,7 +98,7 @@ spin_to_yaw(float target_yaw, car_struct_t *car_struct)
         }
     }
 
-    car_struct->p_right_motor->p_pid->use_pid = true;
+    car_struct->p_pid->use_pid = true;
     vTaskDelay(pdMS_TO_TICKS(50));
 }
 

frtos/motor/motor_init.h

@@ -19,29 +19,32 @@
 
 #include "motor_config.h"
 
-//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,
-//                          .p_pid.kp_value      = 600.f,
-//                          .p_pid.ki_value      = 66.67f,
-//                          .p_pid.kd_value      = 1350.f,};
-
 void
-motor_init(car_struct_t * car_struct)
+motor_init(car_struct_t *car_struct)
 {
     // Semaphore
     g_left_sem  = xSemaphoreCreateBinary();
     g_right_sem = xSemaphoreCreateBinary();
 
+    car_struct->p_pid->use_pid  = true;
+    car_struct->p_pid->kp_value = 600.f;
+    car_struct->p_pid->ki_value = 66.67f;
+    car_struct->p_pid->kd_value = 1350.f;
+
+    // initialize the car_struct
+    car_struct->p_left_motor->pwm.level         = 0u;
+    car_struct->p_left_motor->pwm.channel       = PWM_CHAN_A;
+    car_struct->p_left_motor->speed.distance_cm = 0.0f;
+    car_struct->p_left_motor->p_sem             = &g_left_sem;
+    car_struct->p_left_motor->use_pid           = &car_struct->p_pid->use_pid;
+
+    car_struct->p_right_motor->pwm.level         = 0u;
+    car_struct->p_right_motor->pwm.channel       = PWM_CHAN_B;
+    car_struct->p_right_motor->speed.distance_cm = 0.0f;
+    car_struct->p_right_motor->p_sem             = &g_right_sem;
+    car_struct->p_right_motor->use_pid           = &car_struct->p_pid->use_pid;
+
+    // Initialize speed pins as inputs
     gpio_init(SPEED_PIN_RIGHT);
     gpio_init(SPEED_PIN_LEFT);
     gpio_set_dir(SPEED_PIN_RIGHT, GPIO_IN);
@@ -62,10 +65,10 @@ 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
+    car_struct->p_left_motor->pwm.slice_num
-        (PWM_PIN_LEFT);
+        = pwm_gpio_to_slice_num(PWM_PIN_LEFT);
-    car_struct->p_right_motor->pwm.slice_num = pwm_gpio_to_slice_num
+    car_struct->p_right_motor->pwm.slice_num
-        (PWM_PIN_RIGHT);
+        = pwm_gpio_to_slice_num(PWM_PIN_RIGHT);
 
     // NOTE: PWM clock is 125MHz for raspberrypi pico w by default
 

frtos/motor/motor_pid.h

@@ -31,9 +31,9 @@ compute_pid(float *integral, float *prev_error, car_struct_t *car_struct)
     float derivative = error - *prev_error;
 
     float control_signal
-        = car_struct->p_right_motor->p_pid->kp_value * error
+        = car_struct->p_pid->kp_value * error
-          + car_struct->p_right_motor->p_pid->ki_value * (*integral)
+          + car_struct->p_pid->ki_value * (*integral)
-          + car_struct->p_right_motor->p_pid->kd_value * derivative;
+          + car_struct->p_pid->kd_value * derivative;
 
     *prev_error = error;
 
@@ -48,7 +48,7 @@ repeating_pid_handler(struct repeating_timer *t)
     static float integral   = 0.0f;
     static float prev_error = 0.0f;
 
-    if (!car_strut->p_right_motor->p_pid->use_pid)
+    if (!car_strut->p_pid->use_pid)
     {
         return true;
     }

frtos/motor/motor_speed.h

@@ -54,7 +54,7 @@ monitor_wheel_speed_task(void *pvParameters)
     for (;;)
     {
         if ((xSemaphoreTake(*p_motor->p_sem, pdMS_TO_TICKS(100))
-            == pdTRUE) && (p_motor->p_pid->use_pid == true))
+            == pdTRUE) && (*p_motor->use_pid == true))
         {
             curr_time    = time_us_64();
             elapsed_time = curr_time - prev_time;

frtos/motor/motor_test.c

@@ -23,30 +23,15 @@ motor_control_task(void *params)
 }
 
 void
-launch(car_struct_t *car_strut)
+launch(car_struct_t *car_struct, void *isr_handler)
 {
-    // isr to detect right motor slot
-    gpio_set_irq_enabled(SPEED_PIN_RIGHT, GPIO_IRQ_EDGE_FALL, true);
-    gpio_add_raw_irq_handler(SPEED_PIN_RIGHT, h_wheel_sensor_isr_handler);
-
-    // isr to detect left motor slot
-    gpio_set_irq_enabled(SPEED_PIN_LEFT, GPIO_IRQ_EDGE_FALL, true);
-    gpio_add_raw_irq_handler(SPEED_PIN_LEFT, h_wheel_sensor_isr_handler);
-
-    irq_set_enabled(IO_IRQ_BANK0, true);
-
-    // PID timer
-    struct repeating_timer pid_timer;
-    add_repeating_timer_ms(-50, repeating_pid_handler, car_strut, &pid_timer);
-    //    add_repeating_timer_ms(-50, repeating_pid_handler, NULL, &pid_timer);
-
     // Left wheel
     //
     TaskHandle_t h_monitor_left_wheel_speed_task_handle = NULL;
     xTaskCreate(monitor_wheel_speed_task,
                 "monitor_left_wheel_speed_task",
                 configMINIMAL_STACK_SIZE,
-                (void *)car_strut->p_left_motor,
+                (void *)car_struct->p_left_motor,
                 WHEEL_SPEED_PRIO,
                 &h_monitor_left_wheel_speed_task_handle);
 
@@ -56,7 +41,7 @@ launch(car_struct_t *car_strut)
     xTaskCreate(monitor_wheel_speed_task,
                 "monitor_wheel_speed_task",
                 configMINIMAL_STACK_SIZE,
-                (void *)car_strut->p_right_motor,
+                (void *)car_struct->p_right_motor,
                 WHEEL_SPEED_PRIO,
                 &h_monitor_right_wheel_speed_task_handle);
 
@@ -65,11 +50,19 @@ launch(car_struct_t *car_strut)
     xTaskCreate(motor_control_task,
                 "motor_turning_task",
                 configMINIMAL_STACK_SIZE,
-                (void *)car_strut,
+                (void *)car_struct,
                 WHEEL_CONTROL_PRIO,
                 &h_motor_turning_task_handle);
 
-    vTaskStartScheduler();
+    // isr to detect right motor slot
+    gpio_set_irq_enabled(SPEED_PIN_RIGHT, GPIO_IRQ_EDGE_FALL, true);
+    gpio_add_raw_irq_handler(SPEED_PIN_RIGHT, isr_handler);
+
+    // isr to detect left motor slot
+    gpio_set_irq_enabled(SPEED_PIN_LEFT, GPIO_IRQ_EDGE_FALL, true);
+    gpio_add_raw_irq_handler(SPEED_PIN_LEFT, isr_handler);
+
+    irq_set_enabled(IO_IRQ_BANK0, true);
 }
 
 int
@@ -80,39 +73,23 @@ main(void)
     sleep_ms(4000);
     printf("Test started!\n");
 
-    motor_pid_t g_pid = {
+    motor_t     g_motor_right;
-        .kp_value = 600.f,
+    motor_t     g_motor_left;
-        .ki_value = 66.67f,
+    motor_pid_t g_pid;
-        .kd_value = 1350.f,
-        .use_pid  = true,
-    };
 
-    motor_t g_motor_left = {
+    car_struct_t car_struct = { .p_right_motor = &g_motor_right,
-        .pwm.level         = 0u,
-        .pwm.channel       = PWM_CHAN_A,
-        .speed.distance_cm = 0.0f,
-        .p_sem             = &g_left_sem,
-        .p_pid             = &g_pid,
-    };
-
-    motor_t g_motor_right = {
-        .pwm.level         = 0u,
-        .pwm.channel       = PWM_CHAN_B,
-        .speed.distance_cm = 0.0f,
-        .p_sem             = &g_right_sem,
-        .p_pid             = &g_pid,
-    };
-
-    car_struct_t car_struct = {
                                 .p_left_motor  = &g_motor_left,
-        .p_right_motor = &g_motor_right,
+                                .p_pid         = &g_pid };
-    };
 
     motor_init(&car_struct);
 
-    launch(&car_struct);
+    launch(&car_struct, &h_wheel_sensor_isr_handler);
 
-    // for(;;);
+    // PID timer
+    struct repeating_timer pid_timer;
+    add_repeating_timer_ms(-50, repeating_pid_handler, &car_struct, &pid_timer);
+
+    vTaskStartScheduler();
 
     return (0);
 }