fix optimization negative angle bug

Co-Authored-By: Amanjyoti Mridha <[email protected]>
RoboMaster-Club · Apr 20, 2024 · 33968b3 · 33968b3
1 parent 0c68d23
commit 33968b3
Show file tree

Hide file tree

Showing 3 changed files with 10 additions and 1 deletion.
diff --git a/src/algo/inc/Swerve_Locomotion.h b/src/algo/inc/Swerve_Locomotion.h
@@ -15,6 +15,8 @@
 #define Azimuth_Gear_Ratio 1.0f
 #define NUMBER_OF_MODULES 4
 
+#define SWERVE_OPTIMIZE
+
 typedef struct
 {
     float speed; // m/s

diff --git a/src/algo/src/Swerve_Locomotion.c b/src/algo/src/Swerve_Locomotion.c
@@ -183,6 +183,11 @@ Module_State_t Optimize_Module_Angle(Module_State_t input_state, float measured_
     }
 
     float curr_angle_normalized = fmodf(measured_angle, 2 * PI); // get normalized current angle
+    if (curr_angle_normalized < 0)
+    {
+        curr_angle_normalized += 2.0f * PI;
+    }
+
     float delta = target_angle - curr_angle_normalized;          // calculate the angle delta
 
     // these conditions essentially create the step function for angles to "snap" too offset from the measured angle to avoid flipping

diff --git a/src/devices/src/dm_motor.c b/src/devices/src/dm_motor.c
@@ -76,7 +76,8 @@ void DM_Motor_Disable_Motor(DM_Motor_Handle_t *motor)
     uint8_t *data = motor->can_instance->tx_buffer;
     switch (motor->disable_behavior)
     {
-    case DM_MOTOR_ZERO_CURRENT:
+    case DM_MOTOR_ZERO_CURRENT: 
+    {
         uint16_t pos_temp, vel_temp, kp_temp, kd_temp, torq_temp;
 
         pos_temp = float_to_uint(0, P_MIN, P_MAX, 16);
@@ -94,6 +95,7 @@ void DM_Motor_Disable_Motor(DM_Motor_Handle_t *motor)
         data[6] = ((kd_temp & 0xF) << 4) | (torq_temp >> 8);
         data[7] = torq_temp;
         break;
+    }
     case DM_MOTOR_HARDWARE_DISABLE:
         data[0] = 0xFF;
         data[1] = 0xFF;