Variable Spin Based on Translation Speed (#60)

* prioritize translation over rotation * ramp spin top
RoboMaster-Club · May 28, 2024 · 2bfdf0a · 2bfdf0a
1 parent f342417
commit 2bfdf0a
Show file tree

Hide file tree

Showing 3 changed files with 33 additions and 18 deletions.
diff --git a/src/app/inc/robot.h b/src/app/inc/robot.h
@@ -3,7 +3,8 @@
 
 #include <stdint.h>
 
-typedef struct {
+typedef struct
+{
   uint8_t enabled;
   uint8_t safely_started;
   uint8_t spintop_mode;
@@ -28,7 +29,8 @@ typedef struct {
   float vy_keyboard;
 } Robot_State_t;
 
-typedef struct {
+typedef struct
+{
   uint8_t prev_B;
   uint8_t prev_G;
   uint8_t prev_V;

diff --git a/src/app/src/chassis_task.c b/src/app/src/chassis_task.c
@@ -6,20 +6,45 @@
 #include "imu_task.h"
 #include "Swerve_Locomotion.h"
 
+#define SPINTOP_RAMP_COEF (0.003f)
+#define SPIN_TOP_OMEGA (1.0f)
+
 extern Robot_State_t g_robot_state;
 extern Remote_t g_remote;
 extern IMU_t g_imu;
 
+float chassis_rad;
+
 void Chassis_Task_Init()
 {
     Swerve_Init();
+    chassis_rad = TRACK_WIDTH * 0.5f * sqrtf(2.0f);
 }
 
 void Chassis_Ctrl_Loop()
 {
 
     if (g_robot_state.enabled)
     {
+        /*
+         * 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 = sqrtf(powf(g_robot_state.chassis_x_speed, 2) + powf(g_robot_state.chassis_y_speed, 2));
+            float spin_coeff = chassis_rad * SPIN_TOP_OMEGA / (translation_speed + chassis_rad * SPIN_TOP_OMEGA);
+
+            // ramp up to target omega
+            float target_omega = SPIN_TOP_OMEGA * spin_coeff;
+            g_robot_state.chassis_omega += SPINTOP_RAMP_COEF * (target_omega - g_robot_state.chassis_omega);
+        }
+        else
+        {
+            g_robot_state.chassis_omega *= (1 - SPINTOP_RAMP_COEF);
+        }
+
         Swerve_Drive(g_robot_state.chassis_x_speed, g_robot_state.chassis_y_speed, g_robot_state.chassis_omega);
     }
     else

diff --git a/src/app/src/robot.c b/src/app/src/robot.c
@@ -15,12 +15,10 @@
 #include "ui.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 (1.0f)
 
 Robot_State_t g_robot_state = {0, 0};
 Key_Prev_t g_key_prev = {0};
@@ -49,8 +47,8 @@ void Robot_Init()
     Remote_Init(&huart3);
     CAN_Service_Init();
     Referee_System_Init(&huart1);
-    //Jetson_Orin_Init(&huart6);
-    //  Initialize all tasks
+    // Jetson_Orin_Init(&huart6);
+    //   Initialize all tasks
     Robot_Tasks_Start();
 }
 
@@ -117,24 +115,14 @@ void Robot_Cmd_Loop()
                 g_launch_target.flywheel_enabled = 1;
             }
             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 - 
-                SPINTOP_COEF*(powf(g_robot_state.chassis_y_speed,2)+powf(g_robot_state.chassis_x_speed,2));
-            }
-            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
                 }
             }