From 7ecdf93a104392c53b49be995a4ddb40b64aa524 Mon Sep 17 00:00:00 2001
From: jia-xie <103150184+jia-xie@users.noreply.github.com>
Date: Fri, 2 Aug 2024 02:04:24 -0400
Subject: [PATCH] post-pretest version

---
 src/app/src/chassis_task.c | 14 ++++++++++----
 src/app/src/gimbal_task.c  |  2 +-
 src/app/src/robot.c        | 18 +++++++++---------
 3 files changed, 20 insertions(+), 14 deletions(-)

diff --git a/src/app/src/chassis_task.c b/src/app/src/chassis_task.c
index 295d76a..bd88214 100644
--- a/src/app/src/chassis_task.c
+++ b/src/app/src/chassis_task.c
@@ -157,8 +157,8 @@ void Chassis_Task_Init()
     g_right_foot_motor = MF_Motor_Init(motor_config);
 
     MF_Motor_Broadcast_Init(1);
-    PID_Init(&g_pid_left_leg_length, 8500.0f, 0.0f, 350.0f, 50.0f, 0.0f, 0.0f);
-    PID_Init(&g_pid_right_leg_length, 8500.0f, 0.0f, 350.0f, 50.0f, 0.0f, 0.0f);
+    PID_Init(&g_pid_left_leg_length, 1000.0f, 0.0f, 300.0f, 400.0f, 0.0f, 0.0f);
+    PID_Init(&g_pid_right_leg_length, 1000.0f, 0.0f, 300.0f, 400.0f, 0.0f, 0.0f);
 
     PID_Init(&g_pid_left_leg_angle, 15.0f, 0.0f, 5.75f, 10.0f, 0.0f, 0.0f);
     PID_Init(&g_pid_right_leg_angle, 15.0f, 0.0f, 5.75f, 10.0f, 0.0f, 0.0f);
@@ -172,7 +172,7 @@ void Chassis_Task_Init()
     PID_Init(&g_pid_follow_gimbal, 8.0f, 0.0f, 0.95f, 6.0f, 0.0f, 0.0f);
     g_robot_state.chassis_height = CHASSIS_DEFAULT_HEIGHT;
 
-    PID_Init(&g_pid_roll_compensation, 0.1f, 0.0006f, 0.0f, 0.20f, 0.15f, 0.0f);
+    PID_Init(&g_pid_roll_compensation, 0.4f, 0.0000f, 0.0f, 0.20f, 0.15f, 0.0f);
     xvEstimateKF_Init(&vaEstimateKF);
 }
 
@@ -441,6 +441,12 @@ void _vmc_torq_calc()
     PID_dt(&g_pid_yaw_angle, g_chassis.current_yaw - g_chassis.target_yaw, TASK_TIME);
     g_u_left.T_A -= g_pid_yaw_angle.output;
     g_u_right.T_A += g_pid_yaw_angle.output;
+
+    if (g_robot_state.spintop_mode)
+    {
+        g_u_left.T_A -= (0.1f*(-1.0f));
+        g_u_right.T_A += 0.3f;
+    }
 }
 
 void Chassis_Disable()
@@ -524,7 +530,7 @@ void _chassis_cmd()
         // Spintop vs Follow Gimbal
     if (g_robot_state.spintop_mode)
     {
-        g_chassis.target_yaw_speed = g_chassis.target_yaw_speed * 0.9f + 0.1f * 6.0f;
+        g_chassis.target_yaw_speed = g_chassis.target_yaw_speed * 0.9f + 0.1f * -7.0f;
     }
     else if (g_robot_state.gimbal_switching_dir_pending == 1)
     {
diff --git a/src/app/src/gimbal_task.c b/src/app/src/gimbal_task.c
index 5dd813c..14f11cc 100644
--- a/src/app/src/gimbal_task.c
+++ b/src/app/src/gimbal_task.c
@@ -67,7 +67,7 @@ void Gimbal_Task_Init()
     };
 
     Motor_Config_t pitch_motor_config = {
-        .can_bus = 1,
+        .can_bus = 2,
         .speed_controller_id = 7,
         .offset = 3235,
         .use_external_feedback = 1,
diff --git a/src/app/src/robot.c b/src/app/src/robot.c
index f1c0bf8..bb78810 100644
--- a/src/app/src/robot.c
+++ b/src/app/src/robot.c
@@ -28,7 +28,7 @@ extern DJI_Motor_Handle_t *g_yaw;
 #define MAX_SPEED (1.2f)
 
 Robot_State_t g_robot_state = {0, 0};
-float g_chassis_height_arr[4] = {0.13f, 0.20f, 0.28f, 0.35f};
+float g_chassis_height_arr[6] = {0.15f, 0.18f, 0.21f, 0.24f, 0.27f, 0.30f};
 int8_t g_current_height_index = 0;
 Key_Prev_t g_key_prev = {0};
 extern Launch_Target_t g_launch_target;
@@ -186,7 +186,7 @@ void Robot_Cmd_Loop()
             // this does not interfere with remote control
             if (g_remote.controller.right_switch != UP)
             {
-                // g_robot_state.chassis_height = g_robot_state.chassis_height * 0.99f + 0.01f * g_chassis_height_arr[g_current_height_index];
+                g_robot_state.chassis_height = g_robot_state.chassis_height * 0.99f + 0.01f * g_chassis_height_arr[g_current_height_index];
                 g_robot_state.spintop_mode = 0;
             }
             /* Chassis ends here */
@@ -245,11 +245,11 @@ void Robot_Cmd_Loop()
                     g_launch_target.single_launch_flag = 0;
                     g_launch_target.burst_launch_flag = 1;
                 }
-                // else if ((g_remote.controller.wheel > 50.0f) || (g_remote.mouse.left == 1) || (g_remote.mouse.right == 1))
-                // { // dial wheel backward burst fire
-                //     g_launch_target.single_launch_flag = 0;
-                //     g_launch_target.burst_launch_flag = 1;
-                // }
+                else if ((g_remote.mouse.left == 1) || (g_remote.mouse.right == 1))
+                { // dial wheel backward burst fire
+                    g_launch_target.single_launch_flag = 0;
+                    g_launch_target.burst_launch_flag = 1;
+                }
                 else
                 { // dial wheel mid stop fire
                     g_launch_target.single_launch_flag = 0;
@@ -302,13 +302,13 @@ void Robot_Cmd_Loop()
             {
                 // g_robot_state.chassis_height = g_robot_state.chassis_height * 0.995f + 0.05f * 0.30f;
                 g_current_height_index++;
-                __MAX_LIMIT(g_current_height_index, 0, 3);
+                __MAX_LIMIT(g_current_height_index, 0, 5);
             }
             if (g_remote.keyboard.C == 1 && g_key_prev.prev_C == 0)
             {
                 // g_robot_state.chassis_height = g_robot_state.chassis_height * 0.995f + 0.05f * 0.13f;
                 g_current_height_index--;
-                __MAX_LIMIT(g_current_height_index, 0, 3);
+                __MAX_LIMIT(g_current_height_index, 0, 5);
             }
             if (g_remote.controller.right_switch == MID && g_key_prev.prev_right_switch == UP)
             {