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

obstacle + reverse working

This commit is contained in:
xypoon 2023-11-20 17:38:44 +08:00
parent 082503482b
commit 7ffd8d741c
3 changed files with 38 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
frtos/motor/motor_pid.h
View File

@ -21,6 +21,7 @@ compute_pid(float *integral, float *prev_error, car_struct_t *car_struct)
float error = car_struct->p_left_motor->speed.distance_cm float error = car_struct->p_left_motor->speed.distance_cm
- car_struct->p_right_motor->speed.distance_cm; - car_struct->p_right_motor->speed.distance_cm;
printf("error: %f\n", error);
*integral += error; *integral += error;
float derivative = error - *prev_error; float derivative = error - *prev_error;

2
frtos/motor/motor_speed.h
View File

@ -7,6 +7,7 @@
#define MOTOR_SPEED_H #define MOTOR_SPEED_H
#include "motor_init.h" #include "motor_init.h"
#include "motor_direction.h"
#include "magnetometer_init.h" #include "magnetometer_init.h"
#include "magnetometer_direction.h" #include "magnetometer_direction.h"
@ -119,6 +120,7 @@ distance_to_stop(car_struct_t * pp_car_struct, float distance_cm)
{ {
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); set_wheel_speed_synced(0u, pp_car_struct);
break; break;
} }

58
frtos/rtos_car.c
View File

@ -32,28 +32,34 @@ motor_control_task(void *params)
{ {
car_struct_t *car_struct = (car_struct_t *)params; car_struct_t *car_struct = (car_struct_t *)params;
set_wheel_direction(DIRECTION_FORWARD);
set_wheel_speed_synced(90u, car_struct);
for (;;) for (;;)
{ {
uint8_t temp = check_line_touch(car_struct); uint8_t temp = check_line_touch(car_struct);
switch (temp) switch (temp)
{ {
default: default:
continue; set_wheel_direction(DIRECTION_FORWARD);
set_wheel_speed_synced(90u, car_struct);
distance_to_stop(car_struct, 50.f);
vTaskDelay(pdMS_TO_TICKS(3000));
break;
case 0b01: case 0b01:
car_struct->p_right_motor->speed.current_cms car_struct->p_right_motor->speed.current_cms
+= SLOT_DISTANCE_CM; += SLOT_DISTANCE_CM*1000.f;
break;
case 0b10: case 0b10:
car_struct->p_right_motor->speed.current_cms car_struct->p_right_motor->speed.current_cms
-= SLOT_DISTANCE_CM; -= SLOT_DISTANCE_CM*1000.f;
break;
case 0b11: case 0b11:
vTaskDelay(pdMS_TO_TICKS(1000)); // set_wheel_direction(DIRECTION_MASK);
turn(DIRECTION_LEFT, 90u, 90u, car_struct); // set_wheel_speed_synced(0u, car_struct);
// vTaskDelay(pdMS_TO_TICKS(1000));
// turn(DIRECTION_LEFT, 90u, 90u, car_struct);
break;
} }
vTaskDelay(pdMS_TO_TICKS(50)); vTaskDelay(pdMS_TO_TICKS(25));
} }
} }
@ -69,9 +75,11 @@ obs_task(void *params)
{ {
if (car_struct->obs->ultrasonic_detected) if (car_struct->obs->ultrasonic_detected)
{ {
turn(DIRECTION_LEFT, 180u, 90u, car_struct); // turn(DIRECTION_LEFT, 180u, 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();
distance_to_stop(car_struct, 25.f);
} }
vTaskDelay(pdMS_TO_TICKS(50)); vTaskDelay(pdMS_TO_TICKS(50));
} }
@ -142,22 +150,22 @@ 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,
(void *)&car_struct,
PRIO,
&h_motor_turning_task_handle);
// // obs task
// TaskHandle_t h_obs_task_handle = NULL;
// xTaskCreate(obs_task,
// "obs_task",
// configMINIMAL_STACK_SIZE, // configMINIMAL_STACK_SIZE,
// (void *)&car_struct, // (void *)&car_struct,
// PRIO, // PRIO,
// &h_obs_task_handle); // &h_motor_turning_task_handle);
// obs task
TaskHandle_t h_obs_task_handle = NULL;
xTaskCreate(obs_task,
"obs_task",
configMINIMAL_STACK_SIZE,
(void *)&car_struct,
PRIO,
&h_obs_task_handle);
// PID timer // PID timer
struct repeating_timer pid_timer; struct repeating_timer pid_timer;