diff --git a/Makefile b/Makefile index 010de09..58d430f 100644 --- 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 index 6600bf3..208bde1 100644 --- 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 index bd88214..85a9a36 100644 --- 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,7 +530,12 @@ 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) { @@ -538,7 +543,7 @@ void _chassis_cmd() } 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 index 14f11cc..d2bf2fd 100644 --- 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 index 7b9c562..46f1f22 100644 --- 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 index bb78810..c234a1f 100644 --- 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 new file mode 100644 index 0000000..78d633d --- /dev/null +++ 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 new file mode 100644 index 0000000..bb39b38 --- /dev/null +++ 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); +}