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
- car_struct->p_right_motor->speed.distance_cm;
printf("error: %f\n", error);
*integral += error;
float derivative = error - *prev_error;

2
frtos/motor/motor_speed.h
View File

@ -7,6 +7,7 @@
#define MOTOR_SPEED_H
#include "motor_init.h"
#include "motor_direction.h"
#include "magnetometer_init.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)
{
set_wheel_direction(DIRECTION_MASK);
set_wheel_speed_synced(0u, pp_car_struct);
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;
set_wheel_direction(DIRECTION_FORWARD);
set_wheel_speed_synced(90u, car_struct);
for (;;)
{
uint8_t temp = check_line_touch(car_struct);
switch (temp)
{
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:
car_struct->p_right_motor->speed.current_cms
+= SLOT_DISTANCE_CM;
+= SLOT_DISTANCE_CM*1000.f;
break;
case 0b10:
car_struct->p_right_motor->speed.current_cms
-= SLOT_DISTANCE_CM;
-= SLOT_DISTANCE_CM*1000.f;
break;
case 0b11:
vTaskDelay(pdMS_TO_TICKS(1000));
turn(DIRECTION_LEFT, 90u, 90u, car_struct);
// set_wheel_direction(DIRECTION_MASK);
// 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)
{
turn(DIRECTION_LEFT, 180u, 90u, car_struct);
set_wheel_direction(DIRECTION_FORWARD);
set_wheel_speed_synced(90u, car_struct);
// turn(DIRECTION_LEFT, 180u, 90u, car_struct);
// set_wheel_direction(DIRECTION_FORWARD);
// set_wheel_speed_synced(90u, car_struct);
revert_wheel_direction();
distance_to_stop(car_struct, 25.f);
}
vTaskDelay(pdMS_TO_TICKS(50));
}
@ -142,22 +150,22 @@ main(void)
sleep_ms(1000u);
// control task
TaskHandle_t h_motor_turning_task_handle = NULL;
xTaskCreate(motor_control_task,
"motor_turning_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);
// obs task
TaskHandle_t h_obs_task_handle = NULL;
xTaskCreate(obs_task,
"obs_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,
// (void *)&car_struct,
// PRIO,
// &h_obs_task_handle);
&h_obs_task_handle);
// PID timer
struct repeating_timer pid_timer;