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

format codes

This commit is contained in:
Richie 2023-11-28 13:32:13 +08:00
parent cde1209016
commit d31f4b0ef9
6 changed files with 72 additions and 189 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
frtos/motor/motor_direction.h
View File

@ -49,7 +49,8 @@ revert_wheel_direction()
* @param range acceptable range * @param range acceptable range
* @return true if the current direction is within the range of the target * @return true if the current direction is within the range of the target
*/ */
bool check_direction(float current_direction, float target_direction, float range) bool
check_direction(float current_direction, float target_direction, float range)
{ {
// Normalize directions to be within 0 to 360 degrees // Normalize directions to be within 0 to 360 degrees
current_direction = fmod(current_direction, 360.0f); current_direction = fmod(current_direction, 360.0f);

8
frtos/motor/motor_pid.h
View File

@ -26,17 +26,15 @@ compute_pid(float *integral, float *prev_error, car_struct_t *car_struct)
float derivative = error - *prev_error; float derivative = error - *prev_error;
float control_signal float control_signal = car_struct->p_pid->kp_value * error
= car_struct->p_pid->kp_value * error + car_struct->p_pid->ki_value * (*integral)
+ car_struct->p_pid->ki_value * (*integral) + car_struct->p_pid->kd_value * derivative;
+ car_struct->p_pid->kd_value * derivative;
*prev_error = error; *prev_error = error;
return control_signal; return control_signal;
} }
/*! /*!
* @brief Repeating timer handler for the PID controller * @brief Repeating timer handler for the PID controller
* @param ppp_timer The repeating timer * @param ppp_timer The repeating timer

22
frtos/motor/motor_speed.h
View File

@ -22,8 +22,7 @@ h_wheel_sensor_isr_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);
} }
@ -32,13 +31,11 @@ h_wheel_sensor_isr_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);
} }
} }
/*! /*!
* @brief Task to monitor and control the speed of the wheel * @brief Task to monitor and control the speed of the wheel
* @param ppp_motor motor_speed_t struct pointer * @param ppp_motor motor_speed_t struct pointer
@ -56,15 +53,15 @@ monitor_wheel_speed_task(void *ppp_motor)
for (;;) for (;;)
{ {
if ((xSemaphoreTake(*p_motor->p_sem, pdMS_TO_TICKS(100)) if ((xSemaphoreTake(*p_motor->p_sem, pdMS_TO_TICKS(100)) == pdTRUE)
== pdTRUE) && (*p_motor->use_pid == true)) && (*p_motor->use_pid == true))
{ {
curr_time = time_us_64(); curr_time = time_us_64();
elapsed_time = curr_time - prev_time; elapsed_time = curr_time - prev_time;
prev_time = curr_time; prev_time = curr_time;
p_motor->speed.current_cms p_motor->speed.current_cms
= (float) (SLOT_DISTANCE_CM_MODIFIED / elapsed_time); = (float)(SLOT_DISTANCE_CM_MODIFIED / elapsed_time);
p_motor->speed.distance_cm += SLOT_DISTANCE_CM; p_motor->speed.distance_cm += SLOT_DISTANCE_CM;
} }
@ -112,13 +109,14 @@ set_wheel_speed_synced(uint32_t pwm_level, car_struct_t *pp_car_strut)
* @param distance_cm distance to travel in cm * @param distance_cm distance to travel in cm
*/ */
void void
distance_to_stop(car_struct_t * pp_car_struct, float distance_cm) distance_to_stop(car_struct_t *pp_car_struct, float distance_cm)
{ {
float initial = pp_car_struct->p_left_motor->speed.distance_cm; float initial = pp_car_struct->p_left_motor->speed.distance_cm;
for (;;) for (;;)
{ {
if (pp_car_struct->p_left_motor->speed.distance_cm - initial >= distance_cm) if (pp_car_struct->p_left_motor->speed.distance_cm - initial
>= distance_cm)
{ {
set_wheel_direction(DIRECTION_MASK); set_wheel_direction(DIRECTION_MASK);
set_wheel_speed_synced(0u, pp_car_struct); set_wheel_speed_synced(0u, pp_car_struct);
@ -127,8 +125,8 @@ distance_to_stop(car_struct_t * pp_car_struct, float distance_cm)
vTaskDelay(pdMS_TO_TICKS(10)); vTaskDelay(pdMS_TO_TICKS(10));
} }
vTaskDelay(pdMS_TO_TICKS(1000)); vTaskDelay(pdMS_TO_TICKS(1000));
pp_car_struct->p_right_motor->speed.distance_cm = pp_car_struct->p_right_motor->speed.distance_cm
pp_car_struct->p_left_motor->speed.distance_cm; = pp_car_struct->p_left_motor->speed.distance_cm;
} }
#endif /* MOTOR_SPEED_H */ #endif /* MOTOR_SPEED_H */

2
frtos/motor/motor_test.c
View File

@ -19,8 +19,6 @@ motor_control_task(void *params)
} }
} }
int int
main(void) main(void)
{ {

100
frtos/rtos_car.c
View File

@ -31,14 +31,19 @@ check_collision(void *params)
// return ((car_struct->obs->left_sensor_detected << 1) // return ((car_struct->obs->left_sensor_detected << 1)
// | (car_struct->obs->right_sensor_detected)) // | (car_struct->obs->right_sensor_detected))
// || car_struct->obs->ultrasonic_detected; // || car_struct->obs->ultrasonic_detected;
return check_line_touch(car_struct) || return check_line_touch(car_struct) || car_struct->obs->ultrasonic_detected;
car_struct->obs->ultrasonic_detected;
} }
/*!
* @brief Task for going straight, shift left or right when there is a line,
* by changing the distance of the right wheel, so the speed of the wheel
* will be changed by the PID function.
* @param p_car_struct
*/
void void
motor_control_task(void *params) motor_control_task(void *p_car_struct)
{ {
car_struct_t *car_struct = (car_struct_t *)params; car_struct_t *car_struct = (car_struct_t *)p_car_struct;
set_wheel_direction(DIRECTION_FORWARD); set_wheel_direction(DIRECTION_FORWARD);
set_wheel_speed_synced(90u, car_struct); set_wheel_speed_synced(90u, car_struct);
@ -68,6 +73,11 @@ motor_control_task(void *params)
} }
} }
/*!
* @brief Go forward until there is an obstacle, reverse the wheel direction,
* travel a certain distance, then stop.
* @param p_car_struct
*/
void void
obs_task(void *params) obs_task(void *params)
{ {
@ -80,10 +90,10 @@ obs_task(void *params)
{ {
if (car_struct->obs->ultrasonic_detected) if (car_struct->obs->ultrasonic_detected)
{ {
// turn(DIRECTION_LEFT, 130u, 90u, car_struct); // turn(DIRECTION_LEFT, 130u, 90u, car_struct);
// set_wheel_direction(DIRECTION_FORWARD); // set_wheel_direction(DIRECTION_FORWARD);
// set_wheel_speed_synced(90u, car_struct); // set_wheel_speed_synced(90u, car_struct);
// //
revert_wheel_direction(); revert_wheel_direction();
distance_to_stop(car_struct, 20.f); distance_to_stop(car_struct, 20.f);
} }
@ -91,24 +101,28 @@ obs_task(void *params)
} }
} }
/*!
* @brief Task to demonstrate the turning function using magnetometer
* @param params
*/
void void
turn_task(void *params) turn_task(void *params)
{ {
car_struct_t *car_struct = (car_struct_t *)params; car_struct_t *car_struct = (car_struct_t *)params;
for (;;) for (;;)
{ {
set_wheel_direction(DIRECTION_FORWARD); set_wheel_direction(DIRECTION_FORWARD);
set_wheel_speed_synced(89u, car_struct); set_wheel_speed_synced(89u, car_struct);
distance_to_stop(car_struct, 20.f); distance_to_stop(car_struct, 20.f);
vTaskDelay(pdMS_TO_TICKS(1000)); vTaskDelay(pdMS_TO_TICKS(1000));
// turn_to_yaw(DIRECTION_LEFT, 230.f, 80u, car_struct); // turn_to_yaw(DIRECTION_LEFT, 230.f, 80u, car_struct);
turn(DIRECTION_RIGHT, 80.f, 90u, car_struct); turn(DIRECTION_RIGHT, 80.f, 90u, car_struct);
vTaskDelay(pdMS_TO_TICKS(1000)); vTaskDelay(pdMS_TO_TICKS(1000));
} }
} }
void void
@ -157,11 +171,11 @@ main(void)
// Magnetometer // Magnetometer
magnetometer_init(&car_struct); magnetometer_init(&car_struct);
// magnetometer_tasks_init(&car_struct); // magnetometer_tasks_init(&car_struct);
// updateDirection(car_struct.p_direction); // updateDirection(car_struct.p_direction);
printf("Magnetometer initialized!\n"); printf("Magnetometer initialized!\n");
// sleep_ms(1000); // sleep_ms(1000);
// ultra // ultra
ultrasonic_init(&car_struct); ultrasonic_init(&car_struct);
@ -180,32 +194,32 @@ main(void)
sleep_ms(1000u); sleep_ms(1000u);
// control task // control task
// TaskHandle_t h_motor_turning_task_handle = NULL; // TaskHandle_t h_motor_turning_task_handle = NULL;
// xTaskCreate(motor_control_task, // xTaskCreate(motor_control_task,
// "motor_turning_task", // "motor_turning_task",
// configMINIMAL_STACK_SIZE, // configMINIMAL_STACK_SIZE,
// (void *)&car_struct, // (void *)&car_struct,
// PRIO, // PRIO,
// &h_motor_turning_task_handle); // &h_motor_turning_task_handle);
// obs task // obs task
// TaskHandle_t h_obs_task_handle = NULL; // TaskHandle_t h_obs_task_handle = NULL;
// xTaskCreate(obs_task, // xTaskCreate(obs_task,
// "obs_task", // "obs_task",
// configMINIMAL_STACK_SIZE, // configMINIMAL_STACK_SIZE,
// (void *)&car_struct, // (void *)&car_struct,
// PRIO, // PRIO,
// &h_obs_task_handle); // &h_obs_task_handle);
// turn task // turn task
TaskHandle_t h_turn_task_handle = NULL; TaskHandle_t h_turn_task_handle = NULL;
xTaskCreate(turn_task, xTaskCreate(turn_task,
"turn_task", "turn_task",
configMINIMAL_STACK_SIZE, configMINIMAL_STACK_SIZE,
(void *)&car_struct, (void *)&car_struct,
PRIO, PRIO,
&h_turn_task_handle); &h_turn_task_handle);
// PID timer // PID timer
struct repeating_timer pid_timer; struct repeating_timer pid_timer;

126
frtos/rtos_car_line.c
View File

@ -1,126 +0,0 @@
#include "line_sensor_init.h"
#include "ultrasonic_sensor.h"
#include "car_config.h"
#include "motor_init.h"
/*!
* @brief Check if the car is on the line
* @param params
* @return
*/
uint8_t
check_line_touch(void *params)
{
car_struct_t *car_struct = (car_struct_t *)params;
return (car_struct->obs->left_sensor_detected << 1)
| (car_struct->obs->right_sensor_detected);
}
void
motor_control_task(void *params)
{
car_struct_t *car_struct = (car_struct_t *)params;
set_wheel_direction(DIRECTION_FORWARD);
set_wheel_speed_synced(90u, car_struct);
for (;;)
{
uint8_t temp = check_line_touch(car_struct);
switch (temp)
{
default:
break;
case 0b01: // right
car_struct->p_right_motor->speed.distance_cm -=
SLOT_DISTANCE_CM;
break;
case 0b10: //left
car_struct->p_right_motor->speed.distance_cm +=
SLOT_DISTANCE_CM;
break;
case 0b11:
revert_wheel_direction();
distance_to_stop(car_struct, 17.f);
turn(DIRECTION_LEFT, 90u, 90u, car_struct);
set_wheel_direction(DIRECTION_FORWARD);
set_wheel_speed_synced(90u, car_struct);
break;
}
vTaskDelay(pdMS_TO_TICKS(50));
}
}
void
h_main_irq_handler(void)
{
if (gpio_get_irq_event_mask(SPEED_PIN_LEFT) & GPIO_IRQ_EDGE_FALL)
{
gpio_acknowledge_irq(SPEED_PIN_LEFT, GPIO_IRQ_EDGE_FALL);
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
xSemaphoreGiveFromISR(g_left_sem, &xHigherPriorityTaskWoken);
portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
}
if (gpio_get_irq_event_mask(SPEED_PIN_RIGHT) & GPIO_IRQ_EDGE_FALL)
{
gpio_acknowledge_irq(SPEED_PIN_RIGHT, GPIO_IRQ_EDGE_FALL);
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
xSemaphoreGiveFromISR(g_right_sem, &xHigherPriorityTaskWoken);
portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
}
}
int
main(void)
{
stdio_usb_init();
obs_t obs;
motor_t motor_right;
motor_t motor_left;
motor_pid_t pid;
direction_t direction;
car_struct_t car_struct = { .p_right_motor = &motor_right,
.p_left_motor = &motor_left,
.p_pid = &pid,
.obs = &obs,
.p_direction = &direction };
// line
line_sensor_init(&car_struct);
line_tasks_init(&car_struct);
printf("Line sensor initialized!\n");
// motor
motor_init(&car_struct);
motor_tasks_init(&car_struct, &h_main_irq_handler);
printf("Motor initialized!\n");
sleep_ms(1000u);
// control task
TaskHandle_t h_motor_turning_task_handle = NULL;
xTaskCreate(motor_control_task,
"motor_turning_task",
configMINIMAL_STACK_SIZE,
(void *)&car_struct,
PRIO,
&h_motor_turning_task_handle);
// PID timer
struct repeating_timer pid_timer;
add_repeating_timer_ms(-50, repeating_pid_handler, &car_struct, &pid_timer);
vTaskStartScheduler();
return (0);
}