updated motor with testing codes with line sensor

frtos/CMakeLists.txt

@@ -15,6 +15,7 @@ target_compile_definitions(rtos_car PRIVATE
         NO_SYS=0            # don't want NO_SYS (generally this would be in your lwipopts.h)
         LWIP_SOCKET=1       # we need the socket API (generally this would be in your lwipopts.h)
         PING_USE_SOCKETS=1
+        PICO_MAX_SHARED_IRQ_HANDLERS=5
 )
 target_include_directories(rtos_car PRIVATE
         ${CMAKE_CURRENT_LIST_DIR}/config

frtos/config/motor_config.h

@@ -32,13 +32,17 @@
 #define MAX_SPEED 4900U
 #define MIN_SPEED 0U // To be changed
 
+#define WHEEL_SPEED_PRIO    (tskIDLE_PRIORITY + 1UL)
+#define WHEEL_CONTROL_PRIO  (tskIDLE_PRIORITY + 1UL)
+#define WHEEL_PID_PRIO      (tskIDLE_PRIORITY + 1UL)
+
 /*!
  * @brief Structure for the motor speed parameters
- * @param target_speed_cms Target speed in cm/s
  * @param current_speed_cms Current speed in cm/s
  * @param distance_cm Distance travelled in cm
  */
-typedef struct {
+typedef struct
+{
     float current_cms;
     float distance_cm;
 } motor_speed_t;
@@ -49,7 +53,8 @@ typedef struct {
  * @param pwm_channel PWM channel, either A or B
  * @param pwm_level PWM level, from 0 to 5000
  */
-typedef struct {
+typedef struct
+{
     uint     slice_num;
     uint     channel;
     uint16_t level;
@@ -61,7 +66,8 @@ typedef struct {
  * @param pid_ki Integral gain
  * @param pid_kd Derivative gain
  */
-typedef struct {
+typedef struct
+{
     float kp_value;
     float ki_value;
     float kd_value;
@@ -74,11 +80,19 @@ typedef struct {
  * @param pwm Motor PWM parameters
  * @param pid Motor PID parameters
  */
-typedef struct {
+typedef struct
+{
     motor_speed_t     speed;
     SemaphoreHandle_t sem;
     motor_pwm_t       pwm;
     motor_pid_t       pid;
 } motor_t;
 
+typedef struct
+{
+    float         starting_distance_cm;
+    float         distance_to_travel_cm;
+    volatile bool is_running;
+} distance_to_stop_t;
+
 #endif /* MOTOR_CONFIG_H */

frtos/motor/CMakeLists.txt

@@ -4,12 +4,14 @@ target_link_libraries(motor_test
         pico_cyw43_arch_lwip_sys_freertos
         pico_stdlib
         pico_lwip_iperf
+        hardware_i2c
         FreeRTOS-Kernel-Heap4 # FreeRTOS kernel and dynamic heap
         hardware_pwm
 )
 
 target_include_directories(motor_test PRIVATE
         ../config
+        ../magnetometer
 )
 
 pico_enable_stdio_usb(motor_test 1)

frtos/motor/motor_direction.h

@@ -27,47 +27,88 @@ set_wheel_direction(uint32_t direction)
     gpio_set_mask(direction);
 }
 
-/*!
- * @brief Turn the wheel, must set the priority higher than the motor PID task
- * @param direction The direction of the wheel
- * @param direction_after The direction of the wheel after turning
- * @param speed_after The speed of the wheel after turning
- */
 void
-turn_wheel(uint32_t direction)
+turn_left_90()
 {
-    set_wheel_speed(0u);
+    set_wheel_direction(DIRECTION_RIGHT_FORWARD);
-    vTaskDelay(pdMS_TO_TICKS(1000));
+    set_wheel_speed(3500u, &g_motor_right);
-    float initial_right = g_motor_right.speed.distance_cm;
-    float initial_left  = g_motor_left.speed.distance_cm;
-
-    set_wheel_direction(direction);
-    set_wheel_speed(3500u);
 
+    float initial = g_motor_right.speed.distance_cm;
     for (;;)
     {
-        // gap between wheels = 11.3cm, to turn 90 degrees, need to travel
+        if (g_motor_right.speed.distance_cm - initial >= 17.f)
-        // 11.3 * pi / 4 = 8.9cm
-        if (g_motor_left.speed.distance_cm - initial_left >= 6.8f)
-        {
-            g_motor_left.pwm.level = 0;
-            pwm_set_chan_level(g_motor_left.pwm.slice_num,
-                               g_motor_left.pwm.channel,
-                               g_motor_left.pwm.level);
-        }
-
-        if (g_motor_right.speed.distance_cm - initial_right >= 6.8f)
-        {
-            g_motor_right.pwm.level = 0;
-            pwm_set_chan_level(g_motor_right.pwm.slice_num,
-                               g_motor_right.pwm.channel,
-                               g_motor_right.pwm.level);
-        }
-
-        if (g_motor_left.pwm.level == 0u && g_motor_right.pwm.level == 0u)
         {
+            set_wheel_speed(0u, &g_motor_right);
             break;
         }
+        vTaskDelay(pdMS_TO_TICKS(5));
     }
     vTaskDelay(pdMS_TO_TICKS(1000));
+    g_motor_right.speed.distance_cm = initial;
+    g_motor_left.speed.distance_cm  = initial;
+}
+
+void
+turn_right_90()
+{
+    set_wheel_direction(DIRECTION_LEFT_FORWARD);
+    set_wheel_speed(3500u, &g_motor_left);
+
+    float initial = g_motor_left.speed.distance_cm;
+    for (;;)
+    {
+        if (g_motor_left.speed.distance_cm - initial >= 17.f)
+        {
+            set_wheel_speed(0u, &g_motor_left);
+            break;
+        }
+        vTaskDelay(pdMS_TO_TICKS(5));
+    }
+    vTaskDelay(pdMS_TO_TICKS(1000));
+    g_motor_left.speed.distance_cm  = initial;
+    g_motor_right.speed.distance_cm = initial;
+}
+
+void
+spin_left_90()
+{
+    set_wheel_direction(DIRECTION_LEFT);
+
+    set_wheel_speed_synced(3500u);
+
+    float initial = g_motor_left.speed.distance_cm;
+    for (;;)
+    {
+        if (g_motor_left.speed.distance_cm - initial >= 7.5f)
+        {
+            set_wheel_speed_synced(0u);
+            break;
+        }
+        vTaskDelay(pdMS_TO_TICKS(5));
+    }
+    vTaskDelay(pdMS_TO_TICKS(1000));
+    g_motor_left.speed.distance_cm  = initial;
+    g_motor_right.speed.distance_cm = initial;
+}
+
+void
+spin_right_90()
+{
+    set_wheel_direction(DIRECTION_RIGHT);
+
+    set_wheel_speed_synced(3500u);
+
+    float initial = g_motor_right.speed.distance_cm;
+    for (;;)
+    {
+        if (g_motor_right.speed.distance_cm - initial >= 7.5f)
+        {
+            set_wheel_speed_synced(0u);
+            break;
+        }
+        vTaskDelay(pdMS_TO_TICKS(5));
+    }
+    vTaskDelay(pdMS_TO_TICKS(1000));
+    g_motor_right.speed.distance_cm = initial;
+    g_motor_left.speed.distance_cm  = initial;
 }

frtos/motor/motor_pid.h

@@ -5,7 +5,7 @@
  * @author Richie
  */
 
-#include "motor_init.h"
+// #include "magnetometer_init.h"
 
 /*!
  * @brief Compute the control signal using PID controller
@@ -21,6 +21,8 @@ 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);
+
     *integral += error;
 
     float derivative = error - *prev_error;
@@ -34,22 +36,15 @@ compute_pid(float *integral, float *prev_error)
     return control_signal;
 }
 
-void
+bool
-motor_pid_task(__unused void *p_param)
+repeating_pid_handler(__unused struct repeating_timer *t)
 {
-    float integral   = 0.0f;
+    static float integral   = 0.0f;
-    float prev_error = 0.0f;
+    static float prev_error = 0.0f;
 
-    for (;;)
+    if ((g_motor_left.pwm.level == 0u) || (g_motor_right.pwm.level == 0u))
     {
-        if (g_motor_left.pwm.level == 0u)
+        return true;
-        {
-            g_motor_right.pwm.level = 0;
-            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));
-            continue;
     }
 
     float control_signal = compute_pid(&integral, &prev_error);
@@ -63,15 +58,16 @@ motor_pid_task(__unused void *p_param)
 
     if (temp < MIN_SPEED)
     {
-            temp = MIN_SPEED;
+        temp = MIN_SPEED + 1u;
     }
 
     g_motor_right.pwm.level = (uint16_t)temp;
-
     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));
+    printf("speed: %f cm/s\n", g_motor_right.speed.current_cms);
-    }
+    printf("distance: %f cm\n", g_motor_right.speed.distance_cm);
+
+    return true;
 }

frtos/motor/motor_speed.h

@@ -74,6 +74,26 @@ monitor_wheel_speed_task(void *pvParameters)
     }
 }
 
+void
+set_wheel_speed(uint32_t pwm_level, motor_t * motor)
+{
+    motor->pwm.level = pwm_level;
+
+    pwm_set_chan_level(motor->pwm.slice_num,
+                       motor->pwm.channel,
+                       motor->pwm.level);
+}
+
+/*!
+ * @brief Set the speed of the wheels
+ * @param pwm_level The pwm_level of the wheels, from 0 to 5000
+ */
+void
+set_wheel_speed_synced(uint32_t pwm_level)
+{
+    set_wheel_speed(pwm_level, &g_motor_left);
+    set_wheel_speed(pwm_level, &g_motor_right);
+}
 
 /*!
  * @brief Set the distance to travel before stopping, must be called after
@@ -84,34 +104,16 @@ void
 distance_to_stop(float distance_cm)
 {
     float initial = g_motor_right.speed.distance_cm;
-    printf("initial: %f\n", initial);
 
     for (;;)
     {
         if (g_motor_right.speed.distance_cm - initial >= distance_cm)
         {
-            g_motor_right.pwm.level = 0;
+            set_wheel_speed_synced(0u);
-            g_motor_left.pwm.level  = 0;
             break;
         }
-        // vTaskDelay(pdMS_TO_TICKS(50));
+        vTaskDelay(pdMS_TO_TICKS(10));
     }
-}
+    vTaskDelay(pdMS_TO_TICKS(1000));
-
+    g_motor_right.speed.distance_cm = g_motor_left.speed.distance_cm;
-/*!
- * @brief Set the speed of the wheels
- * @param pwm_level The pwm_level of the wheels, from 0 to 5000
- */
-void
-set_wheel_speed(uint32_t pwm_level)
-{
-    g_motor_right.pwm.level = pwm_level;
-    g_motor_left.pwm.level  = pwm_level;
-
-    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_test.c

@@ -4,37 +4,6 @@
 #include "motor_direction.h"
 #include "motor_pid.h"
 
-#define WHEEL_SPEED_PRIO    (tskIDLE_PRIORITY + 2UL)
-#define WHEEL_CONTROL_PRIO  (tskIDLE_PRIORITY + 2UL)
-#define WHEEL_PID_PRIO      (tskIDLE_PRIORITY + 2UL)
-
-static void
-motor_control_task(__unused void *p_param)
-{
-    for (;;)
-    {
-        set_wheel_direction(DIRECTION_FORWARD);
-        set_wheel_speed(3000u);
-        distance_to_stop(30);
-
-        set_wheel_direction(DIRECTION_BACKWARD);
-        set_wheel_speed(3000u);
-        distance_to_stop(30);
-
-        turn_wheel(DIRECTION_LEFT);
-        set_wheel_direction(DIRECTION_FORWARD);
-        set_wheel_speed(3000u);
-        distance_to_stop(30);
-
-        turn_wheel(DIRECTION_RIGHT);
-        set_wheel_direction(DIRECTION_FORWARD);
-        set_wheel_speed(3000u);
-        distance_to_stop(30);
-
-        set_wheel_speed(0u);
-        vTaskDelay(pdMS_TO_TICKS(3000));
-    }
-}
 
 void
 launch()
@@ -49,8 +18,9 @@ launch()
 
     irq_set_enabled(IO_IRQ_BANK0, true);
 
-//    set_wheel_direction(DIRECTION_FORWARD);
+    // PID timer
-//    set_wheel_speed(3000);
+    struct repeating_timer pid_timer;
+    add_repeating_timer_ms(-50, repeating_pid_handler, NULL, &pid_timer);
 
     // Left wheel
     //
@@ -72,14 +42,6 @@ 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_right,
-                WHEEL_PID_PRIO,
-                &h_motor_pid_right_task_handle);
-
     // control task
     TaskHandle_t h_motor_turning_task_handle = NULL;
     xTaskCreate(motor_control_task,

frtos/rtos_car.c

@@ -14,14 +14,12 @@
 
 #include "motor_speed.h"
 #include "motor_direction.h"
+#include "motor_pid.h"
 
 #include "line_sensor.h"
 
 #include "ultrasonic_sensor.h"
 
-#define READ_LEFT_WHEEL_SPEED_PRIO      (tskIDLE_PRIORITY + 1UL)
-#define READ_RIGHT_WHEEL_SPEED_PRIO     (tskIDLE_PRIORITY + 1UL)
-
 #define READ_LEFT_SENSOR_PRIO    (tskIDLE_PRIORITY + 2UL)
 #define READ_RIGHT_SENSOR_PRIO   (tskIDLE_PRIORITY + 2UL)
 #define READ_BARCODE_SENSOR_PRIO (tskIDLE_PRIORITY + 2UL)
@@ -33,27 +31,73 @@
 /* Common Car State Structure (TODO: TBC)*/
 // static car_state_t g_car_state;
 
+static void
+motor_control_task(__unused void *p_param)
+{
+    static bool left = false;
+    static bool right = false;
+
+    for (;;)
+    {
+        state_t state = gpio_get(LEFT_SENSOR_PIN);
+        printf("state: %d\n", state);
+
+        if (state == 1)
+        {
+            if (!left)
+            {
+                spin_left_90();
+                left = true;
+            }
+            else if (!right)
+            {
+                spin_right_90();
+                spin_right_90();
+                right = true;
+            }
+            else
+            {
+                spin_right_90();
+                left = false;
+                right = false;
+            }
+        }
+        else
+        {
+            set_wheel_direction(DIRECTION_FORWARD);
+            set_wheel_speed_synced(3500u);
+        }
+        vTaskDelay(pdMS_TO_TICKS(5));
+    }
+}
+
 void
 launch()
 {
+//    // isr to detect left line sensor
+//    gpio_set_irq_enabled(LEFT_SENSOR_PIN, GPIO_IRQ_EDGE_FALL, true);
+//    gpio_add_raw_irq_handler(LEFT_SENSOR_PIN, h_line_sensor_handler);
 
-    struct repeating_timer g_left_sensor_timer;
+    // isr to detect right motor slot
-    add_repeating_timer_ms(LINE_SENSOR_READ_DELAY,
+    gpio_set_irq_enabled(SPEED_PIN_RIGHT, GPIO_IRQ_EDGE_FALL, true);
-                           h_left_sensor_timer_handler,
+    gpio_add_raw_irq_handler(SPEED_PIN_RIGHT, h_wheel_sensor_isr_handler);
-                           NULL,
-                           &g_left_sensor_timer);
 
-    struct repeating_timer g_right_sensor_timer;
+    // isr to detect left motor slot
-    add_repeating_timer_ms(LINE_SENSOR_READ_DELAY,
+    gpio_set_irq_enabled(SPEED_PIN_LEFT, GPIO_IRQ_EDGE_FALL, true);
-                           h_right_sensor_timer_handler,
+    gpio_add_raw_irq_handler(SPEED_PIN_LEFT, h_wheel_sensor_isr_handler);
-                           NULL,
-                           &g_right_sensor_timer);
 
-    struct repeating_timer g_barcode_sensor_timer;
+    irq_set_enabled(IO_IRQ_BANK0, true);
-    add_repeating_timer_ms(LINE_SENSOR_READ_DELAY,
+
-                           h_barcode_sensor_timer_handler,
+//    // line sensor timer
-                           NULL,
+//    struct repeating_timer g_left_sensor_timer;
-                           &g_barcode_sensor_timer);
+//    add_repeating_timer_ms(LINE_SENSOR_READ_DELAY,
+//                           h_left_sensor_timer_handler,
+//                           NULL,
+//                           &g_left_sensor_timer);
+
+    // PID timer
+    struct repeating_timer pid_timer;
+    add_repeating_timer_ms(-50, repeating_pid_handler, NULL, &pid_timer);
 
     TaskHandle_t h_monitor_left_sensor_task;
     xTaskCreate(monitor_left_sensor_task,
@@ -63,37 +107,34 @@ launch()
                 READ_LEFT_SENSOR_PRIO,
                 &h_monitor_left_sensor_task);
 
-    TaskHandle_t h_monitor_right_sensor_task;
+    // Left wheel
-    xTaskCreate(monitor_right_sensor_task,
+    //
-                "Monitor Right Sensor Task",
+    TaskHandle_t h_monitor_left_wheel_speed_task_handle = NULL;
+    xTaskCreate(monitor_wheel_speed_task,
+                "monitor_left_wheel_speed_task",
                 configMINIMAL_STACK_SIZE,
-                NULL,
+                (void *)&g_motor_left,
-                READ_RIGHT_SENSOR_PRIO,
+                WHEEL_SPEED_PRIO,
-                &h_monitor_right_sensor_task);
+                &h_monitor_left_wheel_speed_task_handle);
 
-    TaskHandle_t h_monitor_barcode_sensor_task;
+    // Right wheel
-    xTaskCreate(monitor_barcode_sensor_task,
+    //
-                "Monitor Barcode Sensor Task",
+    TaskHandle_t h_monitor_right_wheel_speed_task_handle = NULL;
+    xTaskCreate(monitor_wheel_speed_task,
+                "monitor_wheel_speed_task",
                 configMINIMAL_STACK_SIZE,
-                NULL,
+                (void *)&g_motor_right,
-                READ_BARCODE_SENSOR_PRIO,
+                WHEEL_SPEED_PRIO,
-                &h_monitor_barcode_sensor_task);
+                &h_monitor_right_wheel_speed_task_handle);
 
-    TaskHandle_t h_monitor_direction_task;
+    // control task
-    xTaskCreate(monitor_direction_task,
+    TaskHandle_t h_motor_turning_task_handle = NULL;
-                "Monitor Direction Task",
+    xTaskCreate(motor_control_task,
+                "motor_turning_task",
                 configMINIMAL_STACK_SIZE,
                 NULL,
-                DIRECTION_TASK_PRIORITY,
+                WHEEL_CONTROL_PRIO,
-                &h_monitor_direction_task);
+                &h_motor_turning_task_handle);
-
-    TaskHandle_t h_monitor_distance_task;
-    xTaskCreate(distance_task, 
-                "Monitor Distance Task", 
-                configMINIMAL_STACK_SIZE, 
-                NULL, 
-                DISTANCE_TASK_PRIORITY, 
-                &h_monitor_distance_task);
 
     vTaskStartScheduler();
 }
@@ -103,15 +144,16 @@ main (void)
 {
     stdio_usb_init();
 
-    motor_init();
+    sleep_ms(4000);
+    printf("Test started!\n");
 
-    set_wheel_direction(DIRECTION_LEFT_FORWARD | DIRECTION_RIGHT_FORWARD);
+    motor_init();
 
     line_sensor_setup();
 
-    init_ultrasonic();
+    //init_ultrasonic();
 
-    initialize_car_state(); // TODO: Could be common functionality, To confirm
+    //initialize_car_state(); // TODO: Could be common functionality, To confirm
                             // during Integration
     launch();