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

direction update

This commit is contained in:
Richie 2023-11-09 14:58:35 +08:00
parent 546f340f52
commit 6ac564ee04
1 changed files with 20 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
frtos/motor/motor_direction.h
View File

@ -75,23 +75,9 @@ check_direction(float current_direction, float target_direction, float range)
* @param pp_car_struct The car struct pointer * @param pp_car_struct The car struct pointer
*/ */
void void
spin_to_yaw(float target_yaw, car_struct_t *pp_car_struct) spin_to_yaw(uint32_t direction, float target_yaw, car_struct_t *pp_car_struct)
{ {
updateDirection(); set_wheel_direction(direction);
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_RIGHT);
}
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_LEFT);
}
set_wheel_speed_synced(80u, pp_car_struct); set_wheel_speed_synced(80u, pp_car_struct);
@ -112,4 +98,22 @@ spin_to_yaw(float target_yaw, car_struct_t *pp_car_struct)
vTaskDelay(pdMS_TO_TICKS(50)); vTaskDelay(pdMS_TO_TICKS(50));
} }
void
spin_right(float degree, car_struct_t *pp_car_struct)
{
updateDirection();
float initial_yaw = g_direction.yaw;
float target_yaw = adjust_yaw(initial_yaw + degree);
spin_to_yaw(DIRECTION_RIGHT, target_yaw, pp_car_struct);
}
void
spin_left(float degree, car_struct_t *pp_car_struct)
{
updateDirection();
float initial_yaw = g_direction.yaw;
float target_yaw = adjust_yaw(initial_yaw - degree);
spin_to_yaw(DIRECTION_LEFT, target_yaw, pp_car_struct);
}
#endif /* MOTOR_DIRECTION_H */ #endif /* MOTOR_DIRECTION_H */