From ed516b5ce4be8ad677d65b9e71a9d1ae92e6f947 Mon Sep 17 00:00:00 2001
From: Richie <2837357W@student.gla.ac.uk>
Date: Mon, 30 Oct 2023 23:20:15 +0800
Subject: [PATCH] add direction control

---
 frtos/motor/motor_direction.h | 49 +++++++++++++++++++++++++++--------
 frtos/motor/motor_test.c      | 48 +++++++++++++++++++++++++++++-----
 2 files changed, 80 insertions(+), 17 deletions(-)

diff --git a/frtos/motor/motor_direction.h b/frtos/motor/motor_direction.h
index f81273f..51389fe 100644
--- a/frtos/motor/motor_direction.h
+++ b/frtos/motor/motor_direction.h
@@ -28,19 +28,46 @@ set_wheel_direction(uint32_t direction)
 }
 
 /*!
- * @brief Set the speed of the wheels
- * @param speed The speed of the wheels, from 0 to 5000
+ * @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
-set_wheel_speed(uint32_t speed)
+turn_wheel(uint32_t direction)
 {
-    g_motor_right.pwm.level = speed;
-    g_motor_left.pwm.level  = speed;
+    set_wheel_speed(0u);
+    vTaskDelay(pdMS_TO_TICKS(1000));
+    float initial_right = g_motor_right.speed.distance_cm;
+    float initial_left  = g_motor_left.speed.distance_cm;
 
-    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);
+    set_wheel_direction(direction);
+    set_wheel_speed(3500u);
+
+    for (;;)
+    {
+        // gap between wheels = 11.3cm, to turn 90 degrees, need to travel
+        // 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)
+        {
+            break;
+        }
+    }
+    vTaskDelay(pdMS_TO_TICKS(1000));
 }
\ No newline at end of file
diff --git a/frtos/motor/motor_test.c b/frtos/motor/motor_test.c
index a5f16ff..440972b 100644
--- a/frtos/motor/motor_test.c
+++ b/frtos/motor/motor_test.c
@@ -4,7 +4,37 @@
 #include "motor_direction.h"
 #include "motor_pid.h"
 
-#define WHEEL_SPEED_PRIO (tskIDLE_PRIORITY + 1UL)
+#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()
@@ -19,8 +49,8 @@ launch()
 
     irq_set_enabled(IO_IRQ_BANK0, true);
 
-    // Set wheel speed
-    set_wheel_speed(3500);
+//    set_wheel_direction(DIRECTION_FORWARD);
+//    set_wheel_speed(3000);
 
     // Left wheel
     //
@@ -32,7 +62,6 @@ launch()
                 WHEEL_SPEED_PRIO,
                 &h_monitor_left_wheel_speed_task_handle);
 
-
     // Right wheel
     //
     TaskHandle_t h_monitor_right_wheel_speed_task_handle = NULL;
@@ -48,9 +77,17 @@ launch()
                 "motor_pid_task",
                 configMINIMAL_STACK_SIZE,
                 (void *)&g_motor_right,
-                WHEEL_SPEED_PRIO,
+                WHEEL_PID_PRIO,
                 &h_motor_pid_right_task_handle);
 
+    // control task
+    TaskHandle_t h_motor_turning_task_handle = NULL;
+    xTaskCreate(motor_control_task,
+                "motor_turning_task",
+                configMINIMAL_STACK_SIZE,
+                NULL,
+                WHEEL_CONTROL_PRIO,
+                &h_motor_turning_task_handle);
 
     vTaskStartScheduler();
 }
@@ -64,7 +101,6 @@ main(void)
     printf("Test started!\n");
 
     motor_init();
-    set_wheel_direction(DIRECTION_LEFT_FORWARD | DIRECTION_RIGHT_FORWARD);
 
     launch();