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

add turn to yaw function (pending test)

This commit is contained in:
Richie 2023-11-07 09:35:15 +08:00
parent c19b665e0b
commit b02919bf1b
2 changed files with 41 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
frtos/motor/motor_direction.h
View File

@ -5,6 +5,7 @@
*/ */
#include "motor_init.h" #include "motor_init.h"
#include "magnetometer_init.h"
/*! /*!
* @brief Set the direction of the wheels; can use bitwise OR to set both * @brief Set the direction of the wheels; can use bitwise OR to set both
@ -41,7 +42,7 @@ void
turn_left_90() turn_left_90()
{ {
set_wheel_direction(DIRECTION_RIGHT_FORWARD); set_wheel_direction(DIRECTION_RIGHT_FORWARD);
set_wheel_speed(3500u, &g_motor_right); set_wheel_speed(90u, &g_motor_right);
float initial = g_motor_right.speed.distance_cm; float initial = g_motor_right.speed.distance_cm;
for (;;) for (;;)
@ -62,7 +63,7 @@ void
turn_right_90() turn_right_90()
{ {
set_wheel_direction(DIRECTION_LEFT_FORWARD); set_wheel_direction(DIRECTION_LEFT_FORWARD);
set_wheel_speed(3500u, &g_motor_left); set_wheel_speed(90u, &g_motor_left);
float initial = g_motor_left.speed.distance_cm; float initial = g_motor_left.speed.distance_cm;
for (;;) for (;;)
@ -84,7 +85,7 @@ spin_left_90()
{ {
set_wheel_direction(DIRECTION_LEFT); set_wheel_direction(DIRECTION_LEFT);
set_wheel_speed_synced(3500u); set_wheel_speed_synced(90u);
float initial = g_motor_left.speed.distance_cm; float initial = g_motor_left.speed.distance_cm;
for (;;) for (;;)
@ -106,7 +107,7 @@ spin_right_90()
{ {
set_wheel_direction(DIRECTION_RIGHT); set_wheel_direction(DIRECTION_RIGHT);
set_wheel_speed_synced(3500u); set_wheel_speed_synced(90u);
float initial = g_motor_right.speed.distance_cm; float initial = g_motor_right.speed.distance_cm;
for (;;) for (;;)
@ -122,3 +123,37 @@ spin_right_90()
g_motor_right.speed.distance_cm = initial; g_motor_right.speed.distance_cm = initial;
g_motor_left.speed.distance_cm = initial; g_motor_left.speed.distance_cm = initial;
} }
void
spin_to_yaw(float target_yaw)
{
float initial_yaw = g_direction.yaw;
// if it will to turn more than 180 degrees, turn the other way
if ((target_yaw > initial_yaw) && (target_yaw - initial_yaw < 180.f)
|| ((target_yaw < initial_yaw) && (initial_yaw - target_yaw >= 180.f)))
{
set_wheel_direction(DIRECTION_LEFT);
}
else if ((target_yaw > initial_yaw) && (target_yaw - initial_yaw >= 180.f)
|| ((target_yaw < initial_yaw)
&& (initial_yaw - target_yaw < 180.f)))
{
set_wheel_direction(DIRECTION_RIGHT);
}
set_wheel_speed_synced(90u);
g_use_pid = false;
for (;;)
{
if (initial_yaw == target_yaw)
{
set_wheel_speed_synced(0u);
break;
}
vTaskDelay(pdMS_TO_TICKS(5));
}
g_use_pid = true;
}

4
frtos/motor/motor_speed.h
View File

@ -51,8 +51,8 @@ monitor_wheel_speed_task(void *pvParameters)
for (;;) for (;;)
{ {
if (xSemaphoreTake(p_motor->sem, pdMS_TO_TICKS(100)) if ((xSemaphoreTake(p_motor->sem, pdMS_TO_TICKS(100))
== pdTRUE) == pdTRUE) && (g_use_pid == true))
{ {
curr_time = time_us_64(); curr_time = time_us_64();
elapsed_time = curr_time - prev_time; elapsed_time = curr_time - prev_time;