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
* @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
current_direction = fmod(current_direction, 360.0f);

4
frtos/motor/motor_pid.h
View File

@ -26,8 +26,7 @@ compute_pid(float *integral, float *prev_error, car_struct_t *car_struct)
float derivative = error - *prev_error;
float control_signal
= car_struct->p_pid->kp_value * error
float control_signal = car_struct->p_pid->kp_value * error
+ car_struct->p_pid->ki_value * (*integral)
+ car_struct->p_pid->kd_value * derivative;
@ -36,7 +35,6 @@ 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

18
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);
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
xSemaphoreGiveFromISR(g_left_sem,
&xHigherPriorityTaskWoken);
xSemaphoreGiveFromISR(g_left_sem, &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);
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
xSemaphoreGiveFromISR(g_right_sem,
&xHigherPriorityTaskWoken);
xSemaphoreGiveFromISR(g_right_sem, &xHigherPriorityTaskWoken);
portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
}
}
/*!
* @brief Task to monitor and control the speed of the wheel
* @param ppp_motor motor_speed_t struct pointer
@ -56,8 +53,8 @@ monitor_wheel_speed_task(void *ppp_motor)
for (;;)
{
if ((xSemaphoreTake(*p_motor->p_sem, pdMS_TO_TICKS(100))
== pdTRUE) && (*p_motor->use_pid == true))
if ((xSemaphoreTake(*p_motor->p_sem, pdMS_TO_TICKS(100)) == pdTRUE)
&& (*p_motor->use_pid == true))
{
curr_time = time_us_64();
elapsed_time = curr_time - prev_time;
@ -118,7 +115,8 @@ distance_to_stop(car_struct_t * pp_car_struct, float distance_cm)
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_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(1000));
pp_car_struct->p_right_motor->speed.distance_cm =
pp_car_struct->p_left_motor->speed.distance_cm;
pp_car_struct->p_right_motor->speed.distance_cm
= pp_car_struct->p_left_motor->speed.distance_cm;
}
#endif /* MOTOR_SPEED_H */

2
frtos/motor/motor_test.c
View File

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

22
frtos/rtos_car.c
View File

@ -31,14 +31,19 @@ check_collision(void *params)
// return ((car_struct->obs->left_sensor_detected << 1)
// | (car_struct->obs->right_sensor_detected))
// || car_struct->obs->ultrasonic_detected;
return check_line_touch(car_struct) ||
car_struct->obs->ultrasonic_detected;
return check_line_touch(car_struct) || 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
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_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
obs_task(void *params)
{
@ -91,6 +101,10 @@ obs_task(void *params)
}
}
/*!
* @brief Task to demonstrate the turning function using magnetometer
* @param params
*/
void
turn_task(void *params)
{

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