Updated Turning Functions

Signed-off-by: linuxes_mojoworld@aleeas.com <linuxes_mojoworld@aleeas.com>

frtos/config/magnetometer_config.h

@@ -8,7 +8,7 @@
 #define DIRECTION_READ_DELAY (200)
 
 #define NUM_READINGS \
-    (10) // Number of readings to
+    (5) // Number of readings to
          // take before
          // calculating
          // direction

frtos/magnetometer/magnetometer_direction.h

@@ -316,7 +316,7 @@ updateDirection(volatile direction_t * g_direction)
 
     read_direction(accelerometer, magnetometer, g_direction);
 
-//    print_orientation_data(*g_direction);
+    print_orientation_data(*g_direction);
 //
 //    switch (g_direction->orientation)
 //    {

frtos/motor/motor_direction.h

@@ -10,6 +10,7 @@
 #include "motor_init.h"
 #include "magnetometer_init.h"
 #include "magnetometer_direction.h"
+#include "motor_speed.h"
 #include "math.h"
 
 /*!
@@ -48,21 +49,18 @@ revert_wheel_direction()
  * @param range acceptable range
  * @return true if the current direction is within the range of the target
  */
-bool
+bool check_direction(float current_direction, float target_direction, float range)
-check_direction(float current_direction, float target_direction, float range)
 {
     // Normalize directions to be within 0 to 360 degrees
     current_direction = fmod(current_direction, 360.0f);
-    if (current_direction < 0)
+    current_direction += (current_direction < 0) ? 360.0f : 0.0f;
-        current_direction += 360.0f;
 
     target_direction = fmod(target_direction, 360.0f);
-    if (target_direction < 0)
+    target_direction += (target_direction < 0) ? 360.0f : 0.0f;
-        target_direction += 360.0f;
 
-    // Check if current_direction is within ±1 degree of target_direction
+    // Check if current_direction is within ±range degrees of target_direction
-    if (fabs(current_direction - target_direction) <= range
+    float difference = fabs(current_direction - target_direction);
-        || fabs(current_direction - target_direction) >= (360 - range))
+    if (difference <= range || difference >= (360.0f - range))
     {
         return true;
     }
@@ -91,6 +89,7 @@ turn_to_yaw(uint32_t      direction,
     for (;;)
     {
         updateDirection(pp_car_struct->p_direction);
+        print_orientation_data(*pp_car_struct->p_direction);
         if (check_direction(pp_car_struct->p_direction->yaw, target_yaw, 1))
         {
             set_wheel_direction(DIRECTION_MASK);

frtos/rtos_car.c

@@ -4,15 +4,6 @@
 #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->left_sensor_detected << 1)
-            | (car_struct->obs->right_sensor_detected))
-           || car_struct->obs->ultrasonic_detected;
-}
-
 /*!
  * @brief Check if the car is on the line
  * @param params
@@ -27,6 +18,17 @@ check_line_touch(void *params)
            | (car_struct->obs->right_sensor_detected);
 }
 
+bool
+check_collision(void *params)
+{
+    car_struct_t *car_struct = (car_struct_t *)params;
+    //    return ((car_struct->obs->left_sensor_detected << 1)
+    //            | (car_struct->obs->right_sensor_detected))
+    //           || car_struct->obs->ultrasonic_detected;
+    return check_line_touch(car_struct) ||
+           car_struct->obs->ultrasonic_detected;
+}
+
 void
 motor_control_task(void *params)
 {
@@ -45,11 +47,11 @@ motor_control_task(void *params)
                 break;
             case 0b01:
                 car_struct->p_right_motor->speed.current_cms
-                    += SLOT_DISTANCE_CM*1000.f;
+                    += SLOT_DISTANCE_CM * 1000.f;
                 break;
             case 0b10:
                 car_struct->p_right_motor->speed.current_cms
-                    -= SLOT_DISTANCE_CM*1000.f;
+                    -= SLOT_DISTANCE_CM * 1000.f;
                 break;
             case 0b11:
                 // set_wheel_direction(DIRECTION_MASK);
@@ -85,6 +87,26 @@ obs_task(void *params)
     }
 }
 
+void
+turn_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);
+
+                distance_to_stop(car_struct, 50.f);
+                vTaskDelay(pdMS_TO_TICKS(1000));
+
+//                turn_to_yaw(DIRECTION_LEFT, 230.f, 80u, car_struct);
+
+                turn(DIRECTION_RIGHT, 50.f, 90u, car_struct);
+                vTaskDelay(pdMS_TO_TICKS(1000));
+        }
+}
+
 void
 h_main_irq_handler(void)
 {
@@ -126,6 +148,14 @@ main(void)
                                 .obs           = &obs,
                                 .p_direction   = &direction };
 
+    // Magnetometer
+    magnetometer_init(&car_struct);
+//        magnetometer_tasks_init(&car_struct);
+    //    updateDirection(car_struct.p_direction);
+    printf("Magnetometer initialized!\n");
+
+//    sleep_ms(1000);
+
     // ultra
     ultrasonic_init(&car_struct);
     ultrasonic_task_init(&car_struct);
@@ -141,12 +171,6 @@ main(void)
     motor_tasks_init(&car_struct, &h_main_irq_handler);
     printf("Motor initialized!\n");
 
-    // Magnetometer
-    magnetometer_init(&car_struct);
-    magnetometer_tasks_init(&car_struct);
-    updateDirection(car_struct.p_direction);
-    printf("Magnetometer initialized!\n");
-
     sleep_ms(1000u);
 
     // control task
@@ -167,6 +191,15 @@ main(void)
                 PRIO,
                 &h_obs_task_handle);
 
+    // turn task
+        TaskHandle_t h_turn_task_handle = NULL;
+        xTaskCreate(turn_task,
+                    "turn_task",
+                    configMINIMAL_STACK_SIZE,
+                    (void *)&car_struct,
+                    PRIO,
+                    &h_turn_task_handle);
+
     // PID timer
     struct repeating_timer pid_timer;
     add_repeating_timer_ms(-50, repeating_pid_handler, &car_struct, &pid_timer);