dynamic desaturation draft

Co-Authored-By: Amanjyoti Mridha <[email protected]>

src/algo/inc/Swerve_Locomotion.h

@@ -41,6 +41,6 @@ typedef struct
 } Module_State_Array_t;
 
 void Swerve_Init(void);
-void Swerve_Drive(float x, float y, float omega);
+void Swerve_Drive(float x, float y, float omega, float max_speed);
 void Swerve_Disable(void);
 #endif

src/algo/src/Swerve_Locomotion.c

@@ -87,16 +87,16 @@ void Swerve_Init()
 }
 
 /* Scale wheel speeds to max possible speed while preserving ratio between modules.*/
-Module_State_Array_t Desaturate_Wheel_Speeds(Module_State_Array_t module_state_array)
-{
+Module_State_Array_t Desaturate_Wheel_Speeds(Module_State_Array_t module_state_array, float max_speed)
+{    
     float highest_speed = fabsf(module_state_array.states[0].speed);
     for (int i = 1; i < NUMBER_OF_MODULES; i++) // start from 1 to find the highest speed
     {
         highest_speed = fmaxf(highest_speed, fabsf(module_state_array.states[i].speed));
     }
     if (highest_speed > 0.01f) // avoid division by zero
     {
-        float desaturation_coefficient = fabsf(SWERVE_MAX_SPEED / highest_speed);
+        float desaturation_coefficient = fabsf(max_speed / highest_speed);
         Module_State_Array_t desaturated_module_states = {0}; // initialize the struct to zero
 
         for (int i = 0; i < NUMBER_OF_MODULES; i++)
@@ -246,38 +246,25 @@ void Set_Desired_States(Module_State_Array_t desired_states)
 }
 
 /**
- * Takes inputs from (-1 to 1) and sets the respective module outputs.
+ * Drives the robot using the given x, y, and omega speeds.
  *
- * @param x: x speed of the robot
- * @param y: y speed of the robot
- * @param omega: angular speed of the robot
+ * @param x: x speed of the robot in meters per second
+ * @param y: y speed of the robot in meters per second
+ * @param omega: angular speed of the robot in radians per second
+ * @param max_speed: max achievable speed the robot should be limited to based on current draw
  */
-void Swerve_Drive(float x, float y, float omega)
+void Swerve_Drive(float x, float y, float omega, float max_speed)
 {
-    x *= SWERVE_MAX_SPEED; // convert to m/s
-    y *= SWERVE_MAX_SPEED;
-    omega *= SWERVE_MAX_ANGLUAR_SPEED; // convert to rad/s
     Chassis_Speeds_t desired_chassis_speeds = {.x = x, .y = y, .omega = omega};
-    Set_Desired_States(Chassis_Speeds_To_Module_States(desired_chassis_speeds));
+    Set_Desired_States(
+        Desaturate_Wheel_Speeds(Chassis_Speeds_To_Module_States(desired_chassis_speeds), max_speed));
 
     for (int i = 0; i < NUMBER_OF_MODULES; i++)
     {
         Set_Module_Output(swerve_modules[i], swerve_modules[i]->module_state);
     }
 }
 
-/* Commands modules to stop moving and reset angle to 0. Should be called on robot enable */
-void Reset_Modules()
-{
-    Module_State_Array_t desired_states = {
-        .states = {
-            {0.0f, 0.0f},
-            {0.0f, 0.0f},
-            {0.0f, 0.0f},
-            {0.0f, 0.0f}}};
-    Set_Desired_States(desired_states);
-}
-
 /**
  * Disables all the motors used in the swerve.
  */

src/app/inc/robot.h

@@ -11,6 +11,7 @@ typedef struct {
   float chassis_x_speed;
   float chassis_y_speed;
   float chassis_omega;
+  float chassis_max_speed;
   float gimbal_pitch_angle;
   float gimbal_yaw_angle;
   float vx;

src/app/src/chassis_task.c

@@ -4,23 +4,56 @@
 #include "robot.h"
 #include "remote.h"
 #include "imu_task.h"
+#include "referee_system.h"
 #include "Swerve_Locomotion.h"
 
 extern Robot_State_t g_robot_state;
 extern Remote_t g_remote;
 extern IMU_t g_imu;
 
+#define CHASSIS_MAX_SPEED_METERS (2.0f)
+#define CHASSIS_POWER_BUFFER_MAX_JOULES (60.0f)
+#define CHASSIS_CURRENT_MAX_AMPS (120.0f)
+#define SPINTOP_COEF (0.003f)
+#define SPIN_TOP_OMEGA (1.0f)
+
 void Chassis_Task_Init()
 {
     Swerve_Init();
 }
 
 void Chassis_Ctrl_Loop()
 {
-
     if (g_robot_state.enabled)
     {
-        Swerve_Drive(g_robot_state.chassis_x_speed, g_robot_state.chassis_y_speed, g_robot_state.chassis_omega);
+        /* power buffer*/
+        float power_buffer_frac = Referee_System.Power_n_Heat.Chassis_Power_Buffer / CHASSIS_POWER_BUFFER_MAX_JOULES;
+        float current_draw_frac = Referee_System.Power_n_Heat.Chassis_Current / CHASSIS_CURRENT_MAX_AMPS;
+        float speed_limitter = (current_draw_frac + pow(power_buffer_frac, 2)) / 2;
+
+        if (current_draw_frac < 0.9f)
+        {
+            g_robot_state.chassis_max_speed = CHASSIS_MAX_SPEED_METERS * speed_limitter;
+        }
+        else
+        {
+            g_robot_state.chassis_max_speed = CHASSIS_MAX_SPEED_METERS;
+        }
+
+        /*
+         * scale spintop omega by inverse of translation speed to prioritize translation
+         * spin_coeff = rw/(v + rw) // r = rad, w = desired omega (spin top omega), v = translational speed
+         * chassis_omega *= spin_coeff
+         */
+        if (g_robot_state.spintop_mode)
+        {
+            float translation_speed = sqrt(pow(g_robot_state.chassis_x_speed, 2) + pow(g_robot_state.chassis_y_speed, 2));
+            float chassis_rad = TRACK_WIDTH * 0.5f * sqrt(2);
+            float spin_coeff = chassis_rad * SPIN_TOP_OMEGA / (translation_speed + chassis_rad * SPIN_TOP_OMEGA);
+            g_robot_state.chassis_omega = SPIN_TOP_OMEGA * spin_coeff;
+        }
+
+        Swerve_Drive(g_robot_state.chassis_x_speed, g_robot_state.chassis_y_speed, g_robot_state.chassis_omega, g_robot_state.chassis_max_speed);
     }
     else
     {

src/app/src/robot.c

@@ -14,12 +14,11 @@
 #include "buzzer.h"
 
 extern DJI_Motor_Handle_t *g_yaw;
-#define SPIN_TOP_OMEGA (1.0f)
 
 #define KEYBOARD_RAMP_COEF (0.004f)
-#define SPINTOP_COEF (0.003f)
 #define CONTROLLER_RAMP_COEF (0.8f)
-#define MAX_SPEED (.6f)
+#define MAX_SPEED (0.6f)
+#define GIMBAL_MAX_PITCH (0.4f)
 
 Robot_State_t g_robot_state = {0, 0};
 Key_Prev_t g_key_prev = {0};
@@ -116,22 +115,12 @@ void Robot_Cmd_Loop()
             }
             g_key_prev.prev_left_switch = g_remote.controller.left_switch;
 
-            if (g_robot_state.spintop_mode)
-            {
-                g_robot_state.chassis_omega = (1 - SPINTOP_COEF) * g_robot_state.chassis_omega + SPINTOP_COEF * SPIN_TOP_OMEGA;
-            }
-            else
-            {
-                g_robot_state.chassis_omega = (1 - SPINTOP_COEF) * g_robot_state.chassis_omega + 0.0f;
-            }
-            /* Chassis ends here */
-
             /* Gimbal starts here */
             if ((g_remote.controller.right_switch == UP) || (g_remote.mouse.right == 1)) // mouse right button auto aim
             {
                 if (g_orin_data.receiving.auto_aiming.yaw != 0 || g_orin_data.receiving.auto_aiming.pitch != 0)
                 {
-                    g_robot_state.gimbal_yaw_angle = (1 - 0.2f) * g_robot_state.gimbal_yaw_angle + (0.2f) * (g_imu.rad.yaw - g_orin_data.receiving.auto_aiming.yaw / 180.0f * PI); // + orin
+                    g_robot_state.gimbal_yaw_angle = (1 - 0.2f) * g_robot_state.gimbal_yaw_angle + (0.2f) * (g_imu.rad.yaw - g_orin_data.receiving.auto_aiming.yaw / 180.0f * PI);         // + orin
                     g_robot_state.gimbal_pitch_angle = (1 - 0.2f) * g_robot_state.gimbal_pitch_angle + (0.2f) * (g_imu.rad.pitch - g_orin_data.receiving.auto_aiming.pitch / 180.0f * PI); // + orin
                 }
             }
@@ -195,18 +184,7 @@ void Robot_Cmd_Loop()
 
             /* Hardware Limits */
             g_robot_state.gimbal_yaw_angle = fmod(g_robot_state.gimbal_yaw_angle, 2 * PI);
-            __MAX_LIMIT(g_robot_state.gimbal_pitch_angle, -0.4f, 0.4f);
-            __MAX_LIMIT(g_robot_state.chassis_x_speed, -MAX_SPEED, MAX_SPEED);
-            __MAX_LIMIT(g_robot_state.chassis_y_speed, -MAX_SPEED, MAX_SPEED);
-
-            /* power buffer*/
-            // float power_buffer = Referee_System.Power_n_Heat.Chassis_Power_Buffer / 60.0f;
-            // if (power_buffer < 0.8f)
-            // {
-            //     g_robot_state.chassis_x_speed *= pow(power_buffer,1);
-            //     g_robot_state.chassis_y_speed *= pow(power_buffer,1);
-            //     g_robot_state.chassis_omega *= pow(power_buffer,1);
-            // }
+            __MAX_LIMIT(g_robot_state.gimbal_pitch_angle, -GIMBAL_MAX_PITCH, GIMBAL_MAX_PITCH);
         }
     }
     else