From c0aa48b58be51267cc069780830b498b44666d75 Mon Sep 17 00:00:00 2001
From: Richie <2837357W@student.gla.ac.uk>
Date: Mon, 20 Nov 2023 11:24:11 +0800
Subject: [PATCH] test

---
 frtos/motor/motor_direction.h | 7 +++++++
 frtos/motor/motor_speed.h     | 3 ---
 frtos/rtos_car.c              | 8 ++++++--
 3 files changed, 13 insertions(+), 5 deletions(-)

diff --git a/frtos/motor/motor_direction.h b/frtos/motor/motor_direction.h
index 00ad331..1ea5101 100644
--- a/frtos/motor/motor_direction.h
+++ b/frtos/motor/motor_direction.h
@@ -27,6 +27,13 @@ set_wheel_direction(uint32_t direction)
     gpio_set_mask(direction);
 }
 
+void
+update_target_yaw(car_struct_t * pp_car_struct)
+{
+    updateDirection(pp_car_struct->p_direction);
+    pp_car_struct->p_direction->target_yaw = pp_car_struct->p_direction->yaw;
+}
+
 /*!
  * @brief Set the direction of the wheel to opposite direction using bit mask
  */
diff --git a/frtos/motor/motor_speed.h b/frtos/motor/motor_speed.h
index 010d170..997c205 100644
--- a/frtos/motor/motor_speed.h
+++ b/frtos/motor/motor_speed.h
@@ -101,9 +101,6 @@ set_wheel_speed(uint32_t pwm_level, motor_t *p_motor)
 void
 set_wheel_speed_synced(uint32_t pwm_level, car_struct_t *pp_car_strut)
 {
-    updateDirection(pp_car_strut->p_direction);
-    pp_car_strut->p_direction->target_yaw = pp_car_strut->p_direction->yaw;
-
     set_wheel_speed(pwm_level, pp_car_strut->p_left_motor);
     set_wheel_speed(pwm_level, pp_car_strut->p_right_motor);
 }
diff --git a/frtos/rtos_car.c b/frtos/rtos_car.c
index 6b54826..17de85f 100644
--- a/frtos/rtos_car.c
+++ b/frtos/rtos_car.c
@@ -23,6 +23,9 @@ void
 motor_control_task(void *params)
 {
     car_struct_t *car_struct = (car_struct_t *)params;
+    update_target_yaw(car_struct);
+    set_wheel_direction(DIRECTION_FORWARD);
+    set_wheel_speed_synced(90u, car_struct);
     for (;;)
     {
         //        printf("Collision: %d\n", check_collision(car_struct));
@@ -45,8 +48,6 @@ motor_control_task(void *params)
         //            set_wheel_direction(DIRECTION_FORWARD);
         //            set_wheel_speed_synced(90u, car_struct);
         //        }
-        set_wheel_direction(DIRECTION_FORWARD);
-        set_wheel_speed_synced(90u, car_struct);
         vTaskDelay(pdMS_TO_TICKS(5));
     }
 }
@@ -110,8 +111,11 @@ main(void)
     // Magnetometer
     magnetometer_init(&car_struct);
     //    magnetometer_tasks_init(&car_struct);
+    updateDirection(car_struct.p_direction);
     printf("Magnetometer initialized!\n");
 
+    sleep_ms(1000u);
+
     // control task
     TaskHandle_t h_motor_turning_task_handle = NULL;
     xTaskCreate(motor_control_task,