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.
* if the motor direction is not set, it will not move.
* @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
set_wheel_direction(uint32_t direction)
@ -43,6 +41,13 @@ revert_wheel_direction()
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
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;
}
/*!
* @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
spin_to_yaw(float target_yaw, car_struct_t *car_struct)
spin_to_yaw(float target_yaw, car_struct_t *pp_car_struct)
{
updateDirection();
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_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 (;;)
{
@ -93,12 +103,12 @@ spin_to_yaw(float target_yaw, car_struct_t *car_struct)
if (check_direction(g_direction.yaw, target_yaw, 1))
{
set_wheel_direction(DIRECTION_MASK);
set_wheel_speed_synced(0u, car_struct);
set_wheel_speed_synced(0u, pp_car_struct);
break;
}
}
car_struct->p_pid->use_pid = true;
pp_car_struct->p_pid->use_pid = true;
vTaskDelay(pdMS_TO_TICKS(50));
}

7
frtos/motor/motor_init.h
View File

@ -19,6 +19,13 @@
#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
motor_init(car_struct_t *car_struct)
{

14
frtos/motor/motor_pid.h
View File

@ -8,14 +8,11 @@
#ifndef MOTOR_PID_H
#define MOTOR_PID_H
// #include "magnetometer_init.h"
/*!
* @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 prev_error The previous error of the PID controller
* @param car_struct The car_struct pointer
* @return The control signal
*/
float
@ -38,10 +35,15 @@ compute_pid(float *integral, float *prev_error, car_struct_t *car_struct)
return control_signal;
}
/*!
* @brief Repeating timer handler for the PID controller
* @param ppp_timer The repeating timer
* @return true
*/
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 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
* @param pvParameters motor_speed_t struct pointer
* @param ppp_motor motor_speed_t struct pointer
* @see motor_speed_t
*/
void
monitor_wheel_speed_task(void *pvParameters)
monitor_wheel_speed_task(void *ppp_motor)
{
volatile motor_t *p_motor = NULL;
p_motor = (motor_t *)pvParameters;
p_motor = (motor_t *)ppp_motor;
uint64_t curr_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
* @param pwm_level The pwm_level of the wheels, from 0 to 99
* @param p_motor The motor to set the speed
*/
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)
{
pwm_level = MAX_PWM_LEVEL;
}
set_wheel_speed(pwm_level, car_strut->p_left_motor);
set_wheel_speed(pwm_level, car_strut->p_right_motor);
p_motor->pwm.level = pwm_level;
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
launch(car_struct_t *car_struct, void *isr_handler)
motor_tasks_init(car_struct_t *pp_car_struct, void *p_isr_handler)
{
// Left wheel
//
@ -31,7 +36,7 @@ launch(car_struct_t *car_struct, void *isr_handler)
xTaskCreate(monitor_wheel_speed_task,
"monitor_left_wheel_speed_task",
configMINIMAL_STACK_SIZE,
(void *)car_struct->p_left_motor,
(void *)pp_car_struct->p_left_motor,
WHEEL_SPEED_PRIO,
&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,
"monitor_wheel_speed_task",
configMINIMAL_STACK_SIZE,
(void *)car_struct->p_right_motor,
(void *)pp_car_struct->p_right_motor,
WHEEL_SPEED_PRIO,
&h_monitor_right_wheel_speed_task_handle);
@ -50,17 +55,17 @@ launch(car_struct_t *car_struct, void *isr_handler)
xTaskCreate(motor_control_task,
"motor_turning_task",
configMINIMAL_STACK_SIZE,
(void *)car_struct,
(void *)pp_car_struct,
WHEEL_CONTROL_PRIO,
&h_motor_turning_task_handle);
// isr to detect right motor slot
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
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);
}
@ -73,17 +78,17 @@ main(void)
sleep_ms(4000);
printf("Test started!\n");
motor_t g_motor_right;
motor_t g_motor_left;
motor_pid_t g_pid;
motor_t motor_right;
motor_t motor_left;
motor_pid_t pid;
car_struct_t car_struct = { .p_right_motor = &g_motor_right,
.p_left_motor = &g_motor_left,
.p_pid = &g_pid };
car_struct_t car_struct = { .p_right_motor = &motor_right,
.p_left_motor = &motor_left,
.p_pid = &pid };
motor_init(&car_struct);
launch(&car_struct, &h_wheel_sensor_isr_handler);
motor_tasks_init(&car_struct, &h_wheel_sensor_isr_handler);
// PID timer
struct repeating_timer pid_timer;