devoalda
/
INF2004_Project
mirror of https://github.com/L4ncelot-R/freeRTOS-car.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

changed pid and speed logic with magnetometer

This commit is contained in:
Richie 2023-11-20 10:19:11 +08:00
parent f04a52a56c
commit f888568559
5 changed files with 65 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
frtos/config/magnetometer_config.h
View File

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

1
frtos/magnetometer/magnetometer_init.h
View File

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

30
frtos/motor/motor_pid.h
View File

@ -8,6 +8,28 @@
#ifndef MOTOR_PID_H #ifndef MOTOR_PID_H
#define 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 * @brief Compute the control signal using PID controller
* @param integral The integral term of the PID controller * @param integral The integral term of the PID controller
@ -18,8 +40,12 @@
float float
compute_pid(float *integral, float *prev_error, car_struct_t *car_struct) 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; *integral += error;

5
frtos/motor/motor_speed.h
View File

@ -7,6 +7,8 @@
#define MOTOR_SPEED_H #define MOTOR_SPEED_H
#include "motor_init.h" #include "motor_init.h"
#include "magnetometer_init.h"
#include "magnetometer_direction.h"
/*! /*!
* @brief Interrupt handler for the wheel sensor * @brief Interrupt handler for the wheel sensor
@ -101,6 +103,9 @@ set_wheel_speed(uint32_t pwm_level, motor_t *p_motor)
void void
set_wheel_speed_synced(uint32_t pwm_level, car_struct_t *pp_car_strut) 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_left_motor);
set_wheel_speed(pwm_level, pp_car_strut->p_right_motor); set_wheel_speed(pwm_level, pp_car_strut->p_right_motor);
} }

59
frtos/rtos_car.c
View File

@ -8,7 +8,8 @@ bool
check_collision(void *params) check_collision(void *params)
{ {
car_struct_t *car_struct = (car_struct_t *)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 void
@ -17,26 +18,28 @@ motor_control_task(void *params)
car_struct_t *car_struct = (car_struct_t *)params; car_struct_t *car_struct = (car_struct_t *)params;
for (;;) for (;;)
{ {
printf("Collision: %d\n", check_collision(car_struct)); // printf("Collision: %d\n", check_collision(car_struct));
if (check_collision(car_struct)) // if (check_collision(car_struct))
{ // {
turn(DIRECTION_LEFT, 90, 80u, car_struct); // turn(DIRECTION_LEFT, 90, 80u, car_struct);
//
// if (check_collision(car_struct)) //// if (check_collision(car_struct))
// { //// {
// turn(180, car_struct); //// turn(180, car_struct);
// ////
// if (check_collision(car_struct)) //// if (check_collision(car_struct))
// { //// {
// turn(90, car_struct); //// turn(90, car_struct);
// } //// }
// } //// }
} // }
else // else
{ // {
set_wheel_direction(DIRECTION_FORWARD); // set_wheel_direction(DIRECTION_FORWARD);
set_wheel_speed_synced(90u, car_struct); // set_wheel_speed_synced(90u, car_struct);
} // }
set_wheel_direction(DIRECTION_FORWARD);
set_wheel_speed_synced(90u, car_struct);
vTaskDelay(pdMS_TO_TICKS(5)); vTaskDelay(pdMS_TO_TICKS(5));
} }
} }
@ -49,8 +52,7 @@ h_main_irq_handler(void)
gpio_acknowledge_irq(SPEED_PIN_LEFT, GPIO_IRQ_EDGE_FALL); gpio_acknowledge_irq(SPEED_PIN_LEFT, GPIO_IRQ_EDGE_FALL);
BaseType_t xHigherPriorityTaskWoken = pdFALSE; BaseType_t xHigherPriorityTaskWoken = pdFALSE;
xSemaphoreGiveFromISR(g_left_sem, xSemaphoreGiveFromISR(g_left_sem, &xHigherPriorityTaskWoken);
&xHigherPriorityTaskWoken);
portYIELD_FROM_ISR(xHigherPriorityTaskWoken); portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
} }
@ -59,8 +61,7 @@ h_main_irq_handler(void)
gpio_acknowledge_irq(SPEED_PIN_RIGHT, GPIO_IRQ_EDGE_FALL); gpio_acknowledge_irq(SPEED_PIN_RIGHT, GPIO_IRQ_EDGE_FALL);
BaseType_t xHigherPriorityTaskWoken = pdFALSE; BaseType_t xHigherPriorityTaskWoken = pdFALSE;
xSemaphoreGiveFromISR(g_right_sem, xSemaphoreGiveFromISR(g_right_sem, &xHigherPriorityTaskWoken);
&xHigherPriorityTaskWoken);
portYIELD_FROM_ISR(xHigherPriorityTaskWoken); portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
} }
} }
@ -81,8 +82,8 @@ main(void)
car_struct_t car_struct = { .p_right_motor = &motor_right, car_struct_t car_struct = { .p_right_motor = &motor_right,
.p_left_motor = &motor_left, .p_left_motor = &motor_left,
.p_pid = &pid, .p_pid = &pid,
.obs = &obs, .obs = &obs,
.p_direction = &direction}; .p_direction = &direction };
// ultra // ultra
ultrasonic_init(&car_struct); ultrasonic_init(&car_struct);
@ -94,14 +95,14 @@ main(void)
line_tasks_init(&car_struct); line_tasks_init(&car_struct);
printf("Line sensor initialized!\n"); printf("Line sensor initialized!\n");
//motor // motor
motor_init(&car_struct); motor_init(&car_struct);
motor_tasks_init(&car_struct, &h_main_irq_handler); motor_tasks_init(&car_struct, &h_main_irq_handler);
printf("Motor initialized!\n"); printf("Motor initialized!\n");
// Magnetometer // Magnetometer
magnetometer_init(&car_struct); magnetometer_init(&car_struct);
// magnetometer_tasks_init(&car_struct); // magnetometer_tasks_init(&car_struct);
printf("Magnetometer initialized!\n"); printf("Magnetometer initialized!\n");
// control task // control task