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

Updated Turning Functions 

Signed-off-by: linuxes_mojoworld@aleeas.com <linuxes_mojoworld@aleeas.com>
This commit is contained in:
linuxes_mojoworld@aleeas.com 2023-11-21 10:57:22 +08:00
parent 7ffd8d741c
commit 8c6512e63f
4 changed files with 70 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
frtos/config/magnetometer_config.h
View File

@ -8,7 +8,7 @@
#define DIRECTION_READ_DELAY (200)
#define NUM_READINGS \
(10) // Number of readings to
(5) // Number of readings to
// take before
// calculating
// direction

2
frtos/magnetometer/magnetometer_direction.h
View File

@ -316,7 +316,7 @@ updateDirection(volatile direction_t * g_direction)
read_direction(accelerometer, magnetometer, g_direction);
// print_orientation_data(*g_direction);
print_orientation_data(*g_direction);
//
// switch (g_direction->orientation)
// {

17
frtos/motor/motor_direction.h
View File

@ -10,6 +10,7 @@
#include "motor_init.h"
#include "magnetometer_init.h"
#include "magnetometer_direction.h"
#include "motor_speed.h"
#include "math.h"
/*!
@ -48,21 +49,18 @@ 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);
if (current_direction < 0)
current_direction += 360.0f;
current_direction += (current_direction < 0) ? 360.0f : 0.0f;
target_direction = fmod(target_direction, 360.0f);
if (target_direction < 0)
target_direction += 360.0f;
target_direction += (target_direction < 0) ? 360.0f : 0.0f;
// Check if current_direction is within ±1 degree of target_direction
if (fabs(current_direction - target_direction) <= range
|| fabs(current_direction - target_direction) >= (360 - range))
// Check if current_direction is within ±range degrees of target_direction
float difference = fabs(current_direction - target_direction);
if (difference <= range || difference >= (360.0f - range))
{
return true;
}
@ -91,6 +89,7 @@ turn_to_yaw(uint32_t direction,
for (;;)
{
updateDirection(pp_car_struct->p_direction);
print_orientation_data(*pp_car_struct->p_direction);
if (check_direction(pp_car_struct->p_direction->yaw, target_yaw, 1))
{
set_wheel_direction(DIRECTION_MASK);

67
frtos/rtos_car.c
View File

@ -4,15 +4,6 @@
#include "car_config.h"
#include "motor_init.h"
bool
check_collision(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))
|| car_struct->obs->ultrasonic_detected;
}
/*!
* @brief Check if the car is on the line
* @param params
@ -27,6 +18,17 @@ check_line_touch(void *params)
| (car_struct->obs->right_sensor_detected);
}
bool
check_collision(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))
// || car_struct->obs->ultrasonic_detected;
return check_line_touch(car_struct) ||
car_struct->obs->ultrasonic_detected;
}
void
motor_control_task(void *params)
{
@ -45,11 +47,11 @@ motor_control_task(void *params)
break;
case 0b01:
car_struct->p_right_motor->speed.current_cms
+= SLOT_DISTANCE_CM*1000.f;
+= SLOT_DISTANCE_CM * 1000.f;
break;
case 0b10:
car_struct->p_right_motor->speed.current_cms
-= SLOT_DISTANCE_CM*1000.f;
-= SLOT_DISTANCE_CM * 1000.f;
break;
case 0b11:
// set_wheel_direction(DIRECTION_MASK);
@ -85,6 +87,26 @@ obs_task(void *params)
}
}
void
turn_task(void *params)
{
car_struct_t *car_struct = (car_struct_t *)params;
for (;;)
{
set_wheel_direction(DIRECTION_FORWARD);
set_wheel_speed_synced(90u, car_struct);
distance_to_stop(car_struct, 50.f);
vTaskDelay(pdMS_TO_TICKS(1000));
// turn_to_yaw(DIRECTION_LEFT, 230.f, 80u, car_struct);
turn(DIRECTION_RIGHT, 50.f, 90u, car_struct);
vTaskDelay(pdMS_TO_TICKS(1000));
}
}
void
h_main_irq_handler(void)
{
@ -126,6 +148,14 @@ main(void)
.obs = &obs,
.p_direction = &direction };
// Magnetometer
magnetometer_init(&car_struct);
// magnetometer_tasks_init(&car_struct);
// updateDirection(car_struct.p_direction);
printf("Magnetometer initialized!\n");
// sleep_ms(1000);
// ultra
ultrasonic_init(&car_struct);
ultrasonic_task_init(&car_struct);
@ -141,12 +171,6 @@ main(void)
motor_tasks_init(&car_struct, &h_main_irq_handler);
printf("Motor initialized!\n");
// Magnetometer
magnetometer_init(&car_struct);
magnetometer_tasks_init(&car_struct);
updateDirection(car_struct.p_direction);
printf("Magnetometer initialized!\n");
sleep_ms(1000u);
// control task
@ -167,6 +191,15 @@ main(void)
PRIO,
&h_obs_task_handle);
// turn task
TaskHandle_t h_turn_task_handle = NULL;
xTaskCreate(turn_task,
"turn_task",
configMINIMAL_STACK_SIZE,
(void *)&car_struct,
PRIO,
&h_turn_task_handle);
// PID timer
struct repeating_timer pid_timer;
add_repeating_timer_ms(-50, repeating_pid_handler, &car_struct, &pid_timer);