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

update comments

This commit is contained in:
Richie 2023-11-09 13:20:36 +08:00
parent 107a142ef6
commit da028e41b1
5 changed files with 71 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
frtos/motor/motor_direction.h
View File

@ -19,8 +19,6 @@
* the same function. * the same function.
* if the motor direction is not set, it will not move. * if the motor direction is not set, it will not move.
* @param direction The direction of the left and right wheels * @param direction The direction of the left and right wheels
* @param left_speed The speed of the left motor, from 0.0 to 1.0
* @param right_speed The speed of the right motor, from 0.0 to 1.0
*/ */
void void
set_wheel_direction(uint32_t direction) set_wheel_direction(uint32_t direction)
@ -43,6 +41,13 @@ revert_wheel_direction()
gpio_set_mask(reverted_direction & DIRECTION_MASK); gpio_set_mask(reverted_direction & DIRECTION_MASK);
} }
/*!
* @brief Check if the current direction is within the range of the target
* @param current_direction current yaw
* @param target_direction target yaw
* @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)
{ {
@ -64,8 +69,13 @@ check_direction(float current_direction, float target_direction, float range)
return false; return false;
} }
/*!
* @brief Spin the car to a certain yaw
* @param target_yaw The target yaw to spin to
* @param pp_car_struct The car struct pointer
*/
void void
spin_to_yaw(float target_yaw, car_struct_t *car_struct) spin_to_yaw(float target_yaw, car_struct_t *pp_car_struct)
{ {
updateDirection(); updateDirection();
float initial_yaw = g_direction.yaw; float initial_yaw = g_direction.yaw;
@ -83,9 +93,9 @@ spin_to_yaw(float target_yaw, car_struct_t *car_struct)
set_wheel_direction(DIRECTION_LEFT); set_wheel_direction(DIRECTION_LEFT);
} }
set_wheel_speed_synced(80u, car_struct); set_wheel_speed_synced(80u, pp_car_struct);
car_struct->p_pid->use_pid = false; pp_car_struct->p_pid->use_pid = false;
for (;;) for (;;)
{ {
@ -93,12 +103,12 @@ spin_to_yaw(float target_yaw, car_struct_t *car_struct)
if (check_direction(g_direction.yaw, target_yaw, 1)) if (check_direction(g_direction.yaw, target_yaw, 1))
{ {
set_wheel_direction(DIRECTION_MASK); set_wheel_direction(DIRECTION_MASK);
set_wheel_speed_synced(0u, car_struct); set_wheel_speed_synced(0u, pp_car_struct);
break; break;
} }
} }
car_struct->p_pid->use_pid = true; pp_car_struct->p_pid->use_pid = true;
vTaskDelay(pdMS_TO_TICKS(50)); vTaskDelay(pdMS_TO_TICKS(50));
} }

7
frtos/motor/motor_init.h
View File

@ -19,6 +19,13 @@
#include "motor_config.h" #include "motor_config.h"
/*!
* @brief Initialize the motor
* @param car_struct The car_struct. Need to have the following fields:\n
* - p_left_motor\n
* - p_right_motor\n
* - p_pid
*/
void void
motor_init(car_struct_t *car_struct) motor_init(car_struct_t *car_struct)
{ {

14
frtos/motor/motor_pid.h
View File

@ -8,14 +8,11 @@
#ifndef MOTOR_PID_H #ifndef MOTOR_PID_H
#define MOTOR_PID_H #define MOTOR_PID_H
// #include "magnetometer_init.h"
/*! /*!
* @brief Compute the control signal using PID controller * @brief Compute the control signal using PID controller
* @param target_speed The target speed of the wheel
* @param current_speed The current speed of the wheel
* @param integral The integral term of the PID controller * @param integral The integral term of the PID controller
* @param prev_error The previous error of the PID controller * @param prev_error The previous error of the PID controller
* @param car_struct The car_struct pointer
* @return The control signal * @return The control signal
*/ */
float float
@ -38,10 +35,15 @@ compute_pid(float *integral, float *prev_error, car_struct_t *car_struct)
return control_signal; return control_signal;
} }
/*!
* @brief Repeating timer handler for the PID controller
* @param ppp_timer The repeating timer
* @return true
*/
bool bool
repeating_pid_handler(struct repeating_timer *t) repeating_pid_handler(struct repeating_timer *ppp_timer)
{ {
car_struct_t *car_strut = (car_struct_t *)t->user_data; car_struct_t *car_strut = (car_struct_t *)ppp_timer->user_data;
static float integral = 0.0f; static float integral = 0.0f;
static float prev_error = 0.0f; static float prev_error = 0.0f;

37
frtos/motor/motor_speed.h
View File

@ -38,14 +38,14 @@ h_wheel_sensor_isr_handler(void)
/*! /*!
* @brief Task to monitor and control the speed of the wheel * @brief Task to monitor and control the speed of the wheel
* @param pvParameters motor_speed_t struct pointer * @param ppp_motor motor_speed_t struct pointer
* @see motor_speed_t * @see motor_speed_t
*/ */
void void
monitor_wheel_speed_task(void *pvParameters) monitor_wheel_speed_task(void *ppp_motor)
{ {
volatile motor_t *p_motor = NULL; volatile motor_t *p_motor = NULL;
p_motor = (motor_t *)pvParameters; p_motor = (motor_t *)ppp_motor;
uint64_t curr_time = 0u; uint64_t curr_time = 0u;
uint64_t prev_time = 0u; uint64_t prev_time = 0u;
@ -76,30 +76,35 @@ monitor_wheel_speed_task(void *pvParameters)
} }
} }
void
set_wheel_speed(uint32_t pwm_level, motor_t * motor)
{
motor->pwm.level = pwm_level;
pwm_set_chan_level(motor->pwm.slice_num,
motor->pwm.channel,
motor->pwm.level);
}
/*! /*!
* @brief Set the speed of the wheels * @brief Set the speed of the wheels
* @param pwm_level The pwm_level of the wheels, from 0 to 99 * @param pwm_level The pwm_level of the wheels, from 0 to 99
* @param p_motor The motor to set the speed
*/ */
void void
set_wheel_speed_synced(uint32_t pwm_level, car_struct_t *car_strut) set_wheel_speed(uint32_t pwm_level, motor_t *p_motor)
{ {
if (pwm_level > MAX_PWM_LEVEL) if (pwm_level > MAX_PWM_LEVEL)
{ {
pwm_level = MAX_PWM_LEVEL; pwm_level = MAX_PWM_LEVEL;
} }
set_wheel_speed(pwm_level, car_strut->p_left_motor); p_motor->pwm.level = pwm_level;
set_wheel_speed(pwm_level, car_strut->p_right_motor);
pwm_set_chan_level(
p_motor->pwm.slice_num, p_motor->pwm.channel, p_motor->pwm.level);
}
/*!
* @brief Set the speed of the wheels
* @param pwm_level The pwm_level of the wheels, from 0 to 99
* @param pp_car_strut The car struct pointer
*/
void
set_wheel_speed_synced(uint32_t pwm_level, car_struct_t *pp_car_strut)
{
set_wheel_speed(pwm_level, pp_car_strut->p_left_motor);
set_wheel_speed(pwm_level, pp_car_strut->p_right_motor);
} }
///*! ///*!

31
frtos/motor/motor_test.c
View File

@ -22,8 +22,13 @@ motor_control_task(void *params)
} }
} }
/*!
* @brief init the tasks for the motor
* @param pp_car_struct The car struct
* @param p_isr_handler The isr handler
*/
void void
launch(car_struct_t *car_struct, void *isr_handler) motor_tasks_init(car_struct_t *pp_car_struct, void *p_isr_handler)
{ {
// Left wheel // Left wheel
// //
@ -31,7 +36,7 @@ launch(car_struct_t *car_struct, void *isr_handler)
xTaskCreate(monitor_wheel_speed_task, xTaskCreate(monitor_wheel_speed_task,
"monitor_left_wheel_speed_task", "monitor_left_wheel_speed_task",
configMINIMAL_STACK_SIZE, configMINIMAL_STACK_SIZE,
(void *)car_struct->p_left_motor, (void *)pp_car_struct->p_left_motor,
WHEEL_SPEED_PRIO, WHEEL_SPEED_PRIO,
&h_monitor_left_wheel_speed_task_handle); &h_monitor_left_wheel_speed_task_handle);
@ -41,7 +46,7 @@ launch(car_struct_t *car_struct, void *isr_handler)
xTaskCreate(monitor_wheel_speed_task, xTaskCreate(monitor_wheel_speed_task,
"monitor_wheel_speed_task", "monitor_wheel_speed_task",
configMINIMAL_STACK_SIZE, configMINIMAL_STACK_SIZE,
(void *)car_struct->p_right_motor, (void *)pp_car_struct->p_right_motor,
WHEEL_SPEED_PRIO, WHEEL_SPEED_PRIO,
&h_monitor_right_wheel_speed_task_handle); &h_monitor_right_wheel_speed_task_handle);
@ -50,17 +55,17 @@ launch(car_struct_t *car_struct, void *isr_handler)
xTaskCreate(motor_control_task, xTaskCreate(motor_control_task,
"motor_turning_task", "motor_turning_task",
configMINIMAL_STACK_SIZE, configMINIMAL_STACK_SIZE,
(void *)car_struct, (void *)pp_car_struct,
WHEEL_CONTROL_PRIO, WHEEL_CONTROL_PRIO,
&h_motor_turning_task_handle); &h_motor_turning_task_handle);
// isr to detect right motor slot // isr to detect right motor slot
gpio_set_irq_enabled(SPEED_PIN_RIGHT, GPIO_IRQ_EDGE_FALL, true); gpio_set_irq_enabled(SPEED_PIN_RIGHT, GPIO_IRQ_EDGE_FALL, true);
gpio_add_raw_irq_handler(SPEED_PIN_RIGHT, isr_handler); gpio_add_raw_irq_handler(SPEED_PIN_RIGHT, p_isr_handler);
// isr to detect left motor slot // isr to detect left motor slot
gpio_set_irq_enabled(SPEED_PIN_LEFT, GPIO_IRQ_EDGE_FALL, true); gpio_set_irq_enabled(SPEED_PIN_LEFT, GPIO_IRQ_EDGE_FALL, true);
gpio_add_raw_irq_handler(SPEED_PIN_LEFT, isr_handler); gpio_add_raw_irq_handler(SPEED_PIN_LEFT, p_isr_handler);
irq_set_enabled(IO_IRQ_BANK0, true); irq_set_enabled(IO_IRQ_BANK0, true);
} }
@ -73,17 +78,17 @@ main(void)
sleep_ms(4000); sleep_ms(4000);
printf("Test started!\n"); printf("Test started!\n");
motor_t g_motor_right; motor_t motor_right;
motor_t g_motor_left; motor_t motor_left;
motor_pid_t g_pid; motor_pid_t pid;
car_struct_t car_struct = { .p_right_motor = &g_motor_right, car_struct_t car_struct = { .p_right_motor = &motor_right,
.p_left_motor = &g_motor_left, .p_left_motor = &motor_left,
.p_pid = &g_pid }; .p_pid = &pid };
motor_init(&car_struct); motor_init(&car_struct);
launch(&car_struct, &h_wheel_sensor_isr_handler); motor_tasks_init(&car_struct, &h_wheel_sensor_isr_handler);
// PID timer // PID timer
struct repeating_timer pid_timer; struct repeating_timer pid_timer;