diff --git a/frtos/config/magnetometer_config.h b/frtos/config/magnetometer_config.h index 6cbc5aa..f3c5817 100644 --- a/frtos/config/magnetometer_config.h +++ b/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; diff --git a/frtos/magnetometer/magnetometer_init.h b/frtos/magnetometer/magnetometer_init.h index ef7a89d..238d046 100644 --- a/frtos/magnetometer/magnetometer_init.h +++ b/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; diff --git a/frtos/motor/motor_pid.h b/frtos/motor/motor_pid.h index 5cce816..e47d7c1 100644 --- a/frtos/motor/motor_pid.h +++ b/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 - - car_struct->p_right_motor->speed.distance_cm; + updateDirection(car_struct->p_direction); + + 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; diff --git a/frtos/motor/motor_speed.h b/frtos/motor/motor_speed.h index 8cce43d..c325042 100644 --- a/frtos/motor/motor_speed.h +++ b/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); } diff --git a/frtos/rtos_car.c b/frtos/rtos_car.c index 6a4b121..b80558e 100644 --- a/frtos/rtos_car.c +++ b/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)); - if (check_collision(car_struct)) - { - turn(DIRECTION_LEFT, 90, 80u, car_struct); - -// if (check_collision(car_struct)) -// { -// turn(180, car_struct); -// -// if (check_collision(car_struct)) -// { -// turn(90, car_struct); -// } -// } - } - else - { - set_wheel_direction(DIRECTION_FORWARD); - set_wheel_speed_synced(90u, 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); + //// + //// if (check_collision(car_struct)) + //// { + //// turn(90, car_struct); + //// } + //// } + // } + // else + // { + // 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)); } } @@ -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, - &xHigherPriorityTaskWoken); + xSemaphoreGiveFromISR(g_left_sem, &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, - &xHigherPriorityTaskWoken); + xSemaphoreGiveFromISR(g_right_sem, &xHigherPriorityTaskWoken); portYIELD_FROM_ISR(xHigherPriorityTaskWoken); } } @@ -81,8 +82,8 @@ main(void) car_struct_t car_struct = { .p_right_motor = &motor_right, .p_left_motor = &motor_left, .p_pid = &pid, - .obs = &obs, - .p_direction = &direction}; + .obs = &obs, + .p_direction = &direction }; // ultra ultrasonic_init(&car_struct); @@ -94,14 +95,14 @@ main(void) line_tasks_init(&car_struct); printf("Line sensor initialized!\n"); - //motor + // motor motor_init(&car_struct); motor_tasks_init(&car_struct, &h_main_irq_handler); printf("Motor initialized!\n"); // Magnetometer magnetometer_init(&car_struct); -// magnetometer_tasks_init(&car_struct); + // magnetometer_tasks_init(&car_struct); printf("Magnetometer initialized!\n"); // control task