From 860805cbeff8925de719fcef72251746e53e2d85 Mon Sep 17 00:00:00 2001 From: Ulysse Cura Date: Tue, 27 May 2025 23:04:41 +0200 Subject: [PATCH] Wrap motor control error between -180 and 180 --- main controller code/src/motion_control.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/main controller code/src/motion_control.c b/main controller code/src/motion_control.c index 0d00ccd..7ea6e1c 100644 --- a/main controller code/src/motion_control.c +++ b/main controller code/src/motion_control.c @@ -29,6 +29,9 @@ void i2c_update_motion_control(void) float target_angle_radian = target_angle / 180.0f * M_PI; float error = target_angle - actual_angle; + if(error > 180) error -= 360; + if(error < -180) error += 360; + float correction = error * GAIN_KD; float target_x_axis_speed = cosf(target_angle_radian) * robot.motion_control_data.x_axis_speed +