rmuc, we will make it to knock out next year!

RoboMaster-Club · Aug 4, 2024 · 015478f · 015478f
1 parent 7ecdf93
commit 015478f
Show file tree

Hide file tree

Showing 8 changed files with 139 additions and 17 deletions.
diff --git a/Makefile b/Makefile
@@ -139,6 +139,7 @@ src/devices/src/remote.c \
 src/devices/src/imu_task.c \
 src/devices/src/referee_system.c \
 src/devices/src/jetson_orin.c \
+src/devices/src/laser.c \
 src/app/src/motor_task.c \
 src/app/src/chassis_task.c \
 src/app/src/gimbal_task.c \

diff --git a/src/algo/src/wlb_lqr_controller.c b/src/algo/src/wlb_lqr_controller.c
@@ -1,5 +1,6 @@
 #include "wlb_lqr_controller.h"
 #include <stdint.h>
+#include <math.h>
 #include "robot.h"
 #define LQR11 (-9.0381f)
 #define LQR12 (-0.8002f)
@@ -72,7 +73,7 @@ extern Robot_State_t g_robot_state;
 float lqr_poly_fit_param[12][4] = 
 {{-40.4426,92.2805,-83.0307,1.9406},
 {22.7498,-17.8524,-5.2327,0.2516},
-{-25.5025,25.6231,-9.5258,0.4409},
+{-25.5025,25.6231,-9.5258,0.4409+1.0f},
 {-106.6486,107.8056,-40.8232,1.8520},
 {115.6430,-73.3241,0.9146,9.4114},
 {16.9954,-12.9700,2.2799,0.8896},
@@ -82,6 +83,85 @@ float lqr_poly_fit_param[12][4] =
 {187.4530,-96.2940,-19.6252,21.2350},
 {942.3723,-975.0634,379.1022,-27.2181},
 {80.8579,-89.0861,37.8765,-4.6313}};
+
+
+// {{-205.9340,182.4261,-81.6013,-2.0630},
+// {-4.0351,3.0252,-9.4435,0.1174},
+// {-17.8698,13.3643,-3.4224,-0.6853},
+// {-75.6744,57.1525,-15.3750,-2.8223},
+// {-234.5798,191.8151,-57.7961,7.9388},
+// {-12.1492,10.1305,-3.1450,0.5385},
+// {-120.1702,130.4549,-58.2512,18.2814},
+// {21.1454,-15.6574,3.7942,1.0837},
+// {-136.4653,110.4869,-32.6252,4.1425},
+// {-558.1383,452.4317,-133.9465,17.1937},
+// {737.0973,-556.4398,145.0014,19.3772},
+// {56.9856,-43.7762,11.7950,0.2071}};
+
+// {{-185.9303,167.0137,-77.5052,-1.5654},
+// {-3.2532,2.3636,-9.2167,0.1093},
+// {-16.0410,11.9739,-3.0571,-0.7204},
+// {-68.0619,51.3700,-13.8595,-2.9649},
+// {-223.9978,182.4823,-54.6638,7.4469},
+// {-9.9488,8.3139,-2.5908,0.4628},
+// {-107.1351,116.3386,-52.0359,15.7367},
+// {18.1150,-13.3660,3.2206,0.9935},
+// {-132.8212,107.1330,-31.4378,3.9437},
+// {-542.4699,438.0650,-128.8755,16.3386},
+// {654.3838,-493.0861,128.0963,20.2368},
+// {48.0822,-36.9396,9.9531,0.0150}};
+
+// {{-189.0658,169.6434,-82.6614,-1.4751},
+// {-2.4117,0.7559,-9.6355,0.1219},
+// {-52.3378,39.0928,-9.9916,-2.2465},
+// {-78.6660,60.5445,-17.8265,-3.1014},
+// {-224.6587,183.2522,-55.0089,7.5690},
+// {-9.8484,8.2483,-2.5706,0.4729},
+// {-93.9751,106.9596,-50.0216,16.3929},
+// {20.2443,-15.1916,3.9384,1.0240},
+// {-423.0503,341.5971,-100.4265,12.6507},
+// {-570.3548,462.1715,-136.9180,17.7981},
+// {676.5543,-510.2762,132.7894,19.7376},
+// {49.9542,-38.4243,10.3764,-0.0364}};
+
+// {{-214.4369,192.1422,-93.2637,-1.4090},
+// {-2.8310,0.9267,-10.8075,0.1500},
+// {-66.2022,49.4913,-12.6657,-2.4422},
+// {-96.9598,74.5361,-21.7641,-3.3074},
+// {-231.2971,189.3706,-57.1827,7.9705},
+// {-9.9426,8.3908,-2.6435,0.5026},
+// {-90.9128,110.8634,-54.8607,18.8002},
+// {25.3090,-19.0486,4.9857,1.1705},
+// {-499.1131,404.0542,-119.2520,15.1210},
+// {-659.0234,535.5100,-159.3205,20.8687},
+// {756.1795,-571.0452,148.8974,18.1456},
+// {56.5955,-43.5863,11.7921,-0.1894}};
+
+// {{-390.9732,345.6023,-157.5780,-0.1152},
+// {-7.6662,4.5979,-17.6171,0.3817},
+// {-161.7810,121.4266,-31.2475,-1.9038},
+// {-264.8276,201.6413,-55.5333,-3.0169},
+// {-200.6764,170.2383,-54.2243,8.3679},
+// {-7.2560,6.7557,-2.4148,0.5966},
+// {10.9650,81.6354,-76.0720,34.1730},
+// {68.5742,-51.5749,13.3616,2.0595},
+// {-787.0193,650.2566,-197.5825,26.1145},
+// {-1210.3059,1004.0606,-307.5440,41.8513},
+// {1144.2013,-870.3319,229.4692,2.8618},
+// {97.0347,-75.2897,20.5907,-1.2532}};
+
+// {{-374.8756,331.7781,-147.7309,1.0628},
+// {-6.8573,4.6111,-15.8647,0.4311},
+// {-169.1987,128.1212,-33.4186,-1.0419},
+// {-276.3509,211.9035,-58.5229,-1.6077},
+// {-202.9786,176.9146,-58.4212,9.3453},
+// {-4.5244,4.6905,-1.8783,0.5231},
+// {435.3883,-176.2513,-48.6391,52.6061},
+// {152.3392,-116.5317,31.3821,2.9173},
+// {-1047.7343,887.8103,-279.9796,39.3492},
+// {-1594.5422,1360.0519,-433.9683,63.3174},
+// {2285.4195,-1749.8911,465.7112,-0.6387},
+// {158.2937,-123.8676,34.3126,-2.4652}};
 void LQR_Output(lqr_u_t *u, lqr_ss_t *state)
 {
 float len = state->leg_len;

diff --git a/src/app/src/chassis_task.c b/src/app/src/chassis_task.c
@@ -166,7 +166,7 @@ void Chassis_Task_Init()
     PID_Init(&g_balance_angle_pid, 600.0f, 0.0f, .065f, 10.0f, 0.0f, 0.0f);
     PID_Init(&g_balance_vel_pid, 10.0f, 0.0f, 0.001f, 10.0f, 0.0f, 0.0f);
 
-    PID_Init(&g_pid_yaw_angle, 5.0f, 0.0f, 1.1f, 10.0f, 0.0f, 0.0f);
+    PID_Init(&g_pid_yaw_angle, 1.3f, 0.0f, 1.1f, 5.0f, 0.0f, 0.0f);
     PID_Init(&g_pid_anti_split, 100.0f, 0.0f, 5.0f, 40.0f, 0.0f, 0.0f);
 
     PID_Init(&g_pid_follow_gimbal, 8.0f, 0.0f, 0.95f, 6.0f, 0.0f, 0.0f);
@@ -530,15 +530,20 @@ 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 * -7.0f;
+        if (g_remote.controller.left_switch == MID)
+        {
+            g_chassis.target_yaw_speed = g_chassis.target_yaw_speed * 0.9f + 0.1f * 4.0f;
+        } else {
+            g_chassis.target_yaw_speed = g_chassis.target_yaw_speed * 0.9f + 0.1f * -4.0f;
+        }
     }
     else if (g_robot_state.gimbal_switching_dir_pending == 1)
     {
         g_chassis.target_yaw_speed = 0 * 0.1f + 0.9f * g_chassis.target_yaw_speed;
     }
     else
     {
-        g_chassis.target_yaw_speed = PID_dt(&g_pid_follow_gimbal, g_chassis.angle_diff, TASK_TIME);
+        g_chassis.target_yaw_speed = g_chassis.target_yaw_speed * 0.95f + 0.05f * PID_dt(&g_pid_follow_gimbal, g_chassis.angle_diff, TASK_TIME);
     }
 
     // this interfere with shooting//g_robot_state.chassis_height += g_remote.controller.wheel / 660.0f * 0.003f;

diff --git a/src/app/src/gimbal_task.c b/src/app/src/gimbal_task.c
@@ -85,7 +85,7 @@ void Gimbal_Task_Init()
         .velocity_pid =
             {
                 .kp = 4500.0f,
-                .ki = 0.8f,
+                .ki = 1.8f,
                 .integral_limit = 4000.0f,
                 .output_limit = GM6020_MAX_CURRENT,
             },

diff --git a/src/app/src/launch_task.c b/src/app/src/launch_task.c
@@ -5,6 +5,7 @@
 #include "imu_task.h"
 #include "user_math.h"
 #include "referee_system.h"
+#include "laser.h"
 
 extern Robot_State_t g_robot_state;
 extern Remote_t g_remote;
@@ -90,6 +91,14 @@ void Launch_Ctrl_Loop() {
 
 void Feed_Angle_Calc()
 {
+    // laser
+    if (g_launch_target.flywheel_enabled)
+    {
+        Laser_On();
+    } else {
+        Laser_Off();
+    }
+
     // Update Counter
     g_launch_target.heat_count++;
     g_launch_target.launch_freq_count++;
@@ -122,7 +131,7 @@ void Feed_Angle_Calc()
             // if (g_launch_target.reverse_burst_launch_pending_flag)
             {
                 // g_launch_target.reverse_burst_launch_pending_flag = 0;
-                g_launch_target.feed_angle -= FEED_1_PROJECTILE_ANGLE;
+                g_launch_target.feed_angle -= FEED_1_PROJECTILE_ANGLE * 0.4f;
                 DJI_Motor_Set_Control_Mode(g_motor_feed, POSITION_CONTROL_TOTAL_ANGLE);
                 DJI_Motor_Set_Angle(g_motor_feed,g_launch_target.feed_angle);
 

diff --git a/src/app/src/robot.c b/src/app/src/robot.c
@@ -16,6 +16,8 @@
 #include "Fusion.h"
 #include "robot_param.h"
 #include "board_comm_task.h"
+#include "laser.h"
+
 extern DJI_Motor_Handle_t *g_yaw;
 #define SPIN_TOP_OMEGA (1.0f)
 #define ROBOT_TASK_PERIOD (0.002f)
@@ -25,7 +27,7 @@ extern DJI_Motor_Handle_t *g_yaw;
 #define KEYBOARD_RAMP_COEF (0.005f)
 #define SPINTOP_COEF (0.003f)
 #define CONTROLLER_RAMP_COEF (0.8f)
-#define MAX_SPEED (1.2f)
+#define MAX_SPEED (.8f)
 
 Robot_State_t g_robot_state = {0, 0};
 float g_chassis_height_arr[6] = {0.15f, 0.18f, 0.21f, 0.24f, 0.27f, 0.30f};
@@ -44,6 +46,8 @@ void _toggle_robot_state(uint8_t *state);
 void Robot_Init()
 {
     Buzzer_Init();
+    HAL_TIM_Base_Start(&htim3);
+    HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_3);
     FusionAhrsInitialise(&g_fusion_ahrs);
     CAN_Service_Init();
     Board_Comm_Task_Init();
@@ -101,11 +105,11 @@ void Robot_Cmd_Loop()
             g_launch_target.flywheel_enabled = 0;
             g_robot_state.gimbal_yaw_angle = g_imu.rad.yaw;
             g_current_height_index = 0;
+            Laser_Off();
         }
         else
         {
             g_robot_state.enabled = 1;
-
             /* Gimbal starts here */ // (Launch enable in last if statement)
             if (g_remote.keyboard.Shift == 1)
             {
@@ -148,11 +152,11 @@ void Robot_Cmd_Loop()
             {
                 g_robot_state.wheel_facing_mode = 0;
             }
-            if (g_remote.mouse.left == 1 && fabs(g_robot_state.chassis_y_speed) < 0.3f)
+            if (g_remote.mouse.left == 1 && fabs(g_robot_state.chassis_y_speed) < 0.1f)
             {
                 g_robot_state.wheel_facing_mode = 1;
             }
-            if (g_remote.mouse.right == 1 && fabs(g_robot_state.chassis_x_speed) < 0.3f)
+            if (g_remote.mouse.right == 1 && fabs(g_robot_state.chassis_x_speed) < 0.1f)
             {
                 g_robot_state.wheel_facing_mode = 0;
             }
@@ -240,7 +244,7 @@ void Robot_Cmd_Loop()
             if (1)
             {
                 g_launch_target.prev_burst_launch_flag = g_launch_target.burst_launch_flag;
-                if (g_remote.controller.left_switch == MID)
+                if (g_remote.controller.left_switch == MID && g_remote.controller.right_switch != UP)
                 { // dial wheel forward single fire
                     g_launch_target.single_launch_flag = 0;
                     g_launch_target.burst_launch_flag = 1;
@@ -256,10 +260,10 @@ void Robot_Cmd_Loop()
                     g_launch_target.single_launch_finished_flag = 0;
                     g_launch_target.burst_launch_flag = 0;
                 }
-                if (g_launch_target.burst_launch_flag == 1)
-                {
-                    g_launch_target.flywheel_enabled = 1;
-                }
+                // if (g_launch_target.burst_launch_flag == 1)
+                // {
+                //     g_launch_target.flywheel_enabled = 1;
+                // }
                 // reverse 1 projectile each time after shooting
                 g_launch_target.prev_reverse_flag = g_launch_target.reverse_flag;
                 if ((g_remote.keyboard.R == 1 && g_key_prev.prev_R == 0) || (!g_launch_target.burst_launch_flag && g_launch_target.prev_burst_launch_flag))
@@ -283,6 +287,10 @@ void Robot_Cmd_Loop()
                 g_launch_target.single_launch_finished_flag = 0;
                 g_launch_target.burst_launch_flag = 0;
             }
+            if (g_remote.controller.left_switch == UP && g_remote.controller.right_switch == UP) // turn off flywheel when spintop is on
+            {
+                g_launch_target.burst_launch_flag = 0;
+            }
             /* Launch control ends here */
 
             /* Keyboard Toggles Start Here */
@@ -312,7 +320,7 @@ void Robot_Cmd_Loop()
             }
             if (g_remote.controller.right_switch == MID && g_key_prev.prev_right_switch == UP)
             {
-                g_robot_state.chassis_height = 0.13f;
+                g_robot_state.chassis_height = 0.15f;
             }
             g_key_prev.prev_B = g_remote.keyboard.B;
             g_key_prev.prev_G = g_remote.keyboard.G;
@@ -356,7 +364,7 @@ void Robot_Cmd_Loop()
     {
         // Ensure controller is down before enabling robot, ensure imu is initialized before enabling robot
 #ifdef MASTER
-        if ((g_remote.controller.right_switch == DOWN) && (g_imu.deg_fusion.pitch != 0.0f) && (g_imu.imu_ready_flag == 1) && (g_remote.controller.left_stick.y != -660) && g_board_comm_first_part_established == 1 && g_board_comm_second_part_established == 1)
+        if ((g_remote.controller.left_switch == DOWN) == (g_remote.controller.right_switch == DOWN) && (g_imu.deg_fusion.pitch != 0.0f) && (g_imu.imu_ready_flag == 1) && (g_remote.controller.left_stick.y != -660) && g_board_comm_first_part_established == 1 && g_board_comm_second_part_established == 1)
 #else
         if ((g_imu.deg_fusion.pitch != 0.0f) && (g_imu.imu_ready_flag == 1))
 #endif

diff --git a/src/devices/inc/laser.h b/src/devices/inc/laser.h
@@ -0,0 +1,7 @@
+#ifndef LASER
+#define LASER
+
+void Laser_On(void);
+void Laser_Off(void);
+
+#endif
diff --git a/src/devices/src/laser.c b/src/devices/src/laser.c
@@ -0,0 +1,12 @@
+#include "laser.h"
+#include "tim.h"
+
+void Laser_On()
+{
+    __HAL_TIM_SetCompare(&htim3, TIM_CHANNEL_3, 8399);
+}
+
+void Laser_Off()
+{
+    __HAL_TIM_SetCompare(&htim3, TIM_CHANNEL_3, 0);
+}