ulysse
/
Robot_Principal_2024-2025
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Wrap motor control error between -180 and 180

This commit is contained in:
Ulysse Cura 2025-05-27 23:04:41 +02:00
parent 96013a465c
commit 860805cbef
1 changed files with 3 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
main controller code/src/motion_control.c
View File

@ -29,6 +29,9 @@ void i2c_update_motion_control(void)
float target_angle_radian = target_angle / 180.0f * M_PI; float target_angle_radian = target_angle / 180.0f * M_PI;
float error = target_angle - actual_angle; float error = target_angle - actual_angle;
if(error > 180) error -= 360;
if(error < -180) error += 360;
float correction = error * GAIN_KD; float correction = error * GAIN_KD;
float target_x_axis_speed = cosf(target_angle_radian) * robot.motion_control_data.x_axis_speed + float target_x_axis_speed = cosf(target_angle_radian) * robot.motion_control_data.x_axis_speed +