From 4ea2674363eac437cf880fe406401bc676b9aca4 Mon Sep 17 00:00:00 2001
From: jia-xie <103150184+jia-xie@users.noreply.github.com>
Date: Sat, 23 Mar 2024 13:21:22 -0400
Subject: [PATCH] power limit

---
 src/algo/src/Swerve_Locomotion.c |  2 ++
 src/app/src/debug_task.c         |  2 +-
 src/app/src/robot.c              | 13 +++++++++++--
 3 files changed, 14 insertions(+), 3 deletions(-)

diff --git a/src/algo/src/Swerve_Locomotion.c b/src/algo/src/Swerve_Locomotion.c
index 7bb0ddd..514271f 100644
--- 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
index a48fc0c..ff77907 100644
--- 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);
 }
\ No newline at end of file
diff --git a/src/app/src/robot.c b/src/app/src/robot.c
index 9206fa2..e7f737a 100644
--- 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