power limit

RoboMaster-Club · Mar 23, 2024 · 4ea2674 · 4ea2674
1 parent 3d8e082
commit 4ea2674
Show file tree

Hide file tree

Showing 3 changed files with 14 additions and 3 deletions.
diff --git a/src/algo/src/Swerve_Locomotion.c b/src/algo/src/Swerve_Locomotion.c
@@ -7,6 +7,8 @@ Swerve_Module_t g_swerve_fl, g_swerve_rl, g_swerve_rr, g_swerve_fr;
 Swerve_Module_t *swerve_modules[NUMBER_OF_MODULES] = {&g_swerve_fl, &g_swerve_rl, &g_swerve_rr, &g_swerve_fr};
 float last_swerve_angle[NUMBER_OF_MODULES] = {0.0f, 0.0f, 0.0f, 0.0f};
 
+// #define SWERVE_OPTIMIZE
+
 /**
  * @brief Inverse kinematics matrix for a 4 module swerve, defined counterclockwise from the front left
  *

diff --git a/src/app/src/debug_task.c b/src/app/src/debug_task.c
@@ -18,5 +18,5 @@ extern Launch_Target_t g_launch_target;
 extern uint64_t t;
 void Debug_Task_Loop(void)
 {
-    printf("/*%f,%f*/", Referee_System.Power_n_Heat.Chassis_Power, Referee_System.Power_n_Heat.Chassis_Power_Buffer);
+    printf("a=%f,b=%d\r\n", Referee_System.Power_n_Heat.Chassis_Power, Referee_System.Power_n_Heat.Chassis_Power_Buffer);
 }
diff --git a/src/app/src/robot.c b/src/app/src/robot.c
@@ -39,7 +39,7 @@ void Robot_Init()
     Remote_Init();
     CAN_Service_Init();
     Referee_System_Init(&huart1);
-    Jetson_Orin_Init(&huart6);
+    //Jetson_Orin_Init(&huart6);
     // Initialize all tasks
     Robot_Tasks_Start();
 }
@@ -119,7 +119,7 @@ void Robot_Cmd_Loop()
             if (g_remote.controller.right_switch == MID)
             {
                 g_robot_state.gimbal_yaw_angle -= (g_remote.controller.right_stick.x / 50000.0f + g_remote.mouse.x / 10000.0f);    // controller and mouse
-                g_robot_state.gimbal_pitch_angle -= (g_remote.controller.right_stick.y / 100000.0f + g_remote.mouse.y / 50000.0f); // controller and mouse
+                g_robot_state.gimbal_pitch_angle -= (g_remote.controller.right_stick.y / 100000.0f - g_remote.mouse.y / 50000.0f); // controller and mouse
             }
             else if ((g_remote.controller.right_switch == UP) || (g_remote.mouse.right == 1)) // mouse right button auto aim
             {
@@ -129,6 +129,7 @@ void Robot_Cmd_Loop()
             /* Gimbal ends here */
 
             /* Launch control starts here */
+            //if (Referee_System.Power_n_Heat.Shooter_1_Heat - Referee_System.)
             if (g_remote.controller.wheel < -50.0f)
             { // dial wheel forward single fire
                 g_launch_target.single_launch_flag = 1;
@@ -175,6 +176,14 @@ void Robot_Cmd_Loop()
             __MAX_LIMIT(g_robot_state.gimbal_pitch_angle, -0.2f, 0.2f);
             __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 *= power_buffer;
+                g_robot_state.chassis_y_speed *= power_buffer;
+                g_robot_state.chassis_omega *= power_buffer;
+            }
         }
     }
     else