changed pid and speed logic with magnetometer

frtos/config/magnetometer_config.h

@@ -75,6 +75,7 @@ typedef struct
     float               roll;
     float               pitch;
     float               yaw;
+    float               target_yaw;
     compass_direction_t orientation;
     angle_t             roll_angle;
     angle_t             pitch_angle;

frtos/magnetometer/magnetometer_init.h

@@ -282,6 +282,7 @@ magnetometer_init(car_struct_t *p_car_struct)
     p_car_struct->p_direction->roll        = 0;
     p_car_struct->p_direction->pitch       = 0;
     p_car_struct->p_direction->yaw         = 0;
+    p_car_struct->p_direction->target_yaw  = 0;
     p_car_struct->p_direction->orientation = NORTH;
     p_car_struct->p_direction->roll_angle  = LEFT;
     p_car_struct->p_direction->pitch_angle = UP;

frtos/motor/motor_pid.h

@@ -8,6 +8,28 @@
 #ifndef MOTOR_PID_H
 #define MOTOR_PID_H
 
+#include "magnetometer_init.h"
+#include "magnetometer_direction.h"
+
+float calculate_yaw_difference(float current_yaw, float set_yaw) {
+    // Normalize yaws to the range [0.0, 359.0]
+    current_yaw = fmod(current_yaw, 360.0);
+    set_yaw = fmod(set_yaw, 360.0);
+
+    // Calculate the direct difference
+    float diff = set_yaw - current_yaw;
+
+    // Adjust the difference to consider the circular nature of yaw values
+    if (diff > 180.0) {
+        diff -= 360.0;
+    } else if (diff < -180.0) {
+        diff += 360.0;
+    }
+
+    return diff;
+}
+
+
 /*!
  * @brief Compute the control signal using PID controller
  * @param integral The integral term of the PID controller
@@ -18,8 +40,12 @@
 float
 compute_pid(float *integral, float *prev_error, car_struct_t *car_struct)
 {
-    float error = car_struct->p_left_motor->speed.distance_cm
+    updateDirection(car_struct->p_direction);
-                  - car_struct->p_right_motor->speed.distance_cm;
+
+    float error = calculate_yaw_difference(car_struct->p_direction->yaw,
+                                           car_struct->p_direction->target_yaw);
+//    float error = car_struct->p_left_motor->speed.distance_cm
+//                  - car_struct->p_right_motor->speed.distance_cm;
 
     *integral += error;
 

frtos/motor/motor_speed.h

@@ -7,6 +7,8 @@
 #define MOTOR_SPEED_H
 
 #include "motor_init.h"
+#include "magnetometer_init.h"
+#include "magnetometer_direction.h"
 
 /*!
  * @brief Interrupt handler for the wheel sensor
@@ -101,6 +103,9 @@ 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);
 }

frtos/rtos_car.c

@@ -8,7 +8,8 @@ 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;
+    return car_struct->obs->line_detected
+           || car_struct->obs->ultrasonic_detected;
 }
 
 void
@@ -17,26 +18,28 @@ motor_control_task(void *params)
     car_struct_t *car_struct = (car_struct_t *)params;
     for (;;)
     {
-        printf("Collision: %d\n", check_collision(car_struct));
+        //        printf("Collision: %d\n", check_collision(car_struct));
-        if (check_collision(car_struct))
-        {
-            turn(DIRECTION_LEFT, 90, 80u, car_struct);
-
         //        if (check_collision(car_struct))
         //        {
-//                turn(180, car_struct);
+        //            turn(DIRECTION_LEFT, 90, 80u, car_struct);
         //
-//                if (check_collision(car_struct))
+        ////            if (check_collision(car_struct))
+        ////            {
+        ////                turn(180, car_struct);
+        ////
+        ////                if (check_collision(car_struct))
+        ////                {
+        ////                    turn(90, car_struct);
+        ////                }
+        ////            }
+        //        }
+        //        else
         //        {
-//                    turn(90, car_struct);
+        //            set_wheel_direction(DIRECTION_FORWARD);
+        //            set_wheel_speed_synced(90u, car_struct);
         //        }
-//            }
-        }
-        else
-        {
         set_wheel_direction(DIRECTION_FORWARD);
         set_wheel_speed_synced(90u, car_struct);
-        }
         vTaskDelay(pdMS_TO_TICKS(5));
     }
 }
@@ -49,8 +52,7 @@ h_main_irq_handler(void)
         gpio_acknowledge_irq(SPEED_PIN_LEFT, GPIO_IRQ_EDGE_FALL);
 
         BaseType_t xHigherPriorityTaskWoken = pdFALSE;
-        xSemaphoreGiveFromISR(g_left_sem,
+        xSemaphoreGiveFromISR(g_left_sem, &xHigherPriorityTaskWoken);
-                              &xHigherPriorityTaskWoken);
         portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
     }
 
@@ -59,8 +61,7 @@ h_main_irq_handler(void)
         gpio_acknowledge_irq(SPEED_PIN_RIGHT, GPIO_IRQ_EDGE_FALL);
 
         BaseType_t xHigherPriorityTaskWoken = pdFALSE;
-        xSemaphoreGiveFromISR(g_right_sem,
+        xSemaphoreGiveFromISR(g_right_sem, &xHigherPriorityTaskWoken);
-                              &xHigherPriorityTaskWoken);
         portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
     }
 }