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

pending dodge test

This commit is contained in:
Richie 2023-11-09 15:16:50 +08:00
parent 6ac564ee04
commit 034ccb47e5
2 changed files with 36 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
frtos/motor/motor_direction.h
View File

@ -77,12 +77,12 @@ check_direction(float current_direction, float target_direction, float range)
void void
spin_to_yaw(uint32_t direction, float target_yaw, car_struct_t *pp_car_struct) spin_to_yaw(uint32_t direction, float target_yaw, car_struct_t *pp_car_struct)
{ {
pp_car_struct->p_pid->use_pid = false;
set_wheel_direction(direction); set_wheel_direction(direction);
set_wheel_speed_synced(80u, pp_car_struct); set_wheel_speed_synced(80u, pp_car_struct);
pp_car_struct->p_pid->use_pid = false;
for (;;) for (;;)
{ {
updateDirection(); updateDirection();
@ -101,18 +101,26 @@ spin_to_yaw(uint32_t direction, float target_yaw, car_struct_t *pp_car_struct)
void void
spin_right(float degree, car_struct_t *pp_car_struct) spin_right(float degree, car_struct_t *pp_car_struct)
{ {
set_wheel_direction(DIRECTION_MASK);
vTaskDelay(pdMS_TO_TICKS(50));
updateDirection(); updateDirection();
float initial_yaw = g_direction.yaw; float initial_yaw = g_direction.yaw;
float target_yaw = adjust_yaw(initial_yaw + degree); float target_yaw = adjust_yaw(initial_yaw + degree);
spin_to_yaw(DIRECTION_RIGHT, target_yaw, pp_car_struct); spin_to_yaw(DIRECTION_RIGHT, target_yaw, pp_car_struct);
} }
void void
spin_left(float degree, car_struct_t *pp_car_struct) spin_left(float degree, car_struct_t *pp_car_struct)
{ {
set_wheel_direction(DIRECTION_MASK);
vTaskDelay(pdMS_TO_TICKS(50));
updateDirection(); updateDirection();
float initial_yaw = g_direction.yaw; float initial_yaw = g_direction.yaw;
float target_yaw = adjust_yaw(initial_yaw - degree); float target_yaw = adjust_yaw(initial_yaw - degree);
spin_to_yaw(DIRECTION_LEFT, target_yaw, pp_car_struct); spin_to_yaw(DIRECTION_LEFT, target_yaw, pp_car_struct);
} }

33
frtos/rtos_car.c
View File

@ -4,21 +4,40 @@
#include "car_config.h" #include "car_config.h"
#include "motor_init.h" #include "motor_init.h"
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;
}
void void
motor_control_task(void *params) motor_control_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); if (check_collision(car_struct))
set_wheel_speed_synced(90u, car_struct); {
spin_left(90, car_struct);
vTaskDelay(pdMS_TO_TICKS(10000)); if (check_collision(car_struct))
{
spin_right(180, car_struct);
revert_wheel_direction(); if (check_collision(car_struct))
set_wheel_speed_synced(90u, car_struct); {
spin_right(90, car_struct);
vTaskDelay(pdMS_TO_TICKS(10000)); }
}
}
else
{
set_wheel_direction(DIRECTION_FORWARD);
set_wheel_speed_synced(90u, car_struct);
}
vTaskDelay(pdMS_TO_TICKS(5));
} }
} }