From 034ccb47e55e98cd8d66eacc9350c7a84bd1d099 Mon Sep 17 00:00:00 2001
From: Richie <2837357W@student.gla.ac.uk>
Date: Thu, 9 Nov 2023 15:16:50 +0800
Subject: [PATCH] pending dodge test

---
 frtos/motor/motor_direction.h | 12 ++++++++++--
 frtos/rtos_car.c              | 33 ++++++++++++++++++++++++++-------
 2 files changed, 36 insertions(+), 9 deletions(-)

diff --git a/frtos/motor/motor_direction.h b/frtos/motor/motor_direction.h
index 6fce7f0..fed0c87 100644
--- a/frtos/motor/motor_direction.h
+++ b/frtos/motor/motor_direction.h
@@ -77,12 +77,12 @@ check_direction(float current_direction, float target_direction, float range)
 void
 spin_to_yaw(uint32_t direction, float target_yaw, car_struct_t *pp_car_struct)
 {
+    pp_car_struct->p_pid->use_pid = false;
+
     set_wheel_direction(direction);
 
     set_wheel_speed_synced(80u, pp_car_struct);
 
-    pp_car_struct->p_pid->use_pid = false;
-
     for (;;)
     {
         updateDirection();
@@ -101,18 +101,26 @@ spin_to_yaw(uint32_t direction, float target_yaw, car_struct_t *pp_car_struct)
 void
 spin_right(float degree, car_struct_t *pp_car_struct)
 {
+    set_wheel_direction(DIRECTION_MASK);
+    vTaskDelay(pdMS_TO_TICKS(50));
+
     updateDirection();
     float initial_yaw = g_direction.yaw;
     float target_yaw  = adjust_yaw(initial_yaw + degree);
+
     spin_to_yaw(DIRECTION_RIGHT, target_yaw, pp_car_struct);
 }
 
 void
 spin_left(float degree, car_struct_t *pp_car_struct)
 {
+    set_wheel_direction(DIRECTION_MASK);
+    vTaskDelay(pdMS_TO_TICKS(50));
+
     updateDirection();
     float initial_yaw = g_direction.yaw;
     float target_yaw  = adjust_yaw(initial_yaw - degree);
+
     spin_to_yaw(DIRECTION_LEFT, target_yaw, pp_car_struct);
 }
 
diff --git a/frtos/rtos_car.c b/frtos/rtos_car.c
index 7e256b6..8627969 100644
--- a/frtos/rtos_car.c
+++ b/frtos/rtos_car.c
@@ -4,21 +4,40 @@
 #include "car_config.h"
 #include "motor_init.h"
 
+bool
+check_collision(void *params)
+{
+    car_struct_t *car_struct = (car_struct_t *)params;
+    return !(car_struct->obs->line_detected) ||
+           car_struct->obs->ultrasonic_detected;
+}
+
 void
 motor_control_task(void *params)
 {
     car_struct_t *car_struct = (car_struct_t *)params;
     for (;;)
     {
-        set_wheel_direction(DIRECTION_FORWARD);
-        set_wheel_speed_synced(90u, car_struct);
+        if (check_collision(car_struct))
+        {
+            spin_left(90, car_struct);
 
-        vTaskDelay(pdMS_TO_TICKS(10000));
+            if (check_collision(car_struct))
+            {
+                spin_right(180, car_struct);
 
-        revert_wheel_direction();
-        set_wheel_speed_synced(90u, car_struct);
-
-        vTaskDelay(pdMS_TO_TICKS(10000));
+                if (check_collision(car_struct))
+                {
+                    spin_right(90, car_struct);
+                }
+            }
+        }
+        else
+        {
+            set_wheel_direction(DIRECTION_FORWARD);
+            set_wheel_speed_synced(90u, car_struct);
+        }
+        vTaskDelay(pdMS_TO_TICKS(5));
     }
 }