From b02919bf1b83d68327623236ebc36bcda675b239 Mon Sep 17 00:00:00 2001
From: Richie <2837357W@student.gla.ac.uk>
Date: Tue, 7 Nov 2023 09:35:15 +0800
Subject: [PATCH] add turn to yaw function (pending test)

---
 frtos/motor/motor_direction.h | 43 +++++++++++++++++++++++++++++++----
 frtos/motor/motor_speed.h     |  4 ++--
 2 files changed, 41 insertions(+), 6 deletions(-)

diff --git a/frtos/motor/motor_direction.h b/frtos/motor/motor_direction.h
index a5ad873..6c31e29 100644
--- a/frtos/motor/motor_direction.h
+++ b/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
@@ -41,7 +42,7 @@ 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 (;;)
@@ -62,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 (;;)
@@ -84,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 (;;)
@@ -106,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 (;;)
@@ -121,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;
 }
\ No newline at end of file
diff --git a/frtos/motor/motor_speed.h b/frtos/motor/motor_speed.h
index 53891be..9b556ec 100644
--- a/frtos/motor/motor_speed.h
+++ b/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))
-            == pdTRUE)
+        if ((xSemaphoreTake(p_motor->sem, pdMS_TO_TICKS(100))
+            == pdTRUE) && (g_use_pid == true))
         {
             curr_time    = time_us_64();
             elapsed_time = curr_time - prev_time;