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 pitch;
float yaw;
float target_yaw;
compass_direction_t orientation;
angle_t roll_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->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;

30
frtos/motor/motor_pid.h
View File

@ -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;

5
frtos/motor/motor_speed.h
View File

@ -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);
}

59
frtos/rtos_car.c
View File

@ -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