From 321a45b999d21563d1e88d74bc6469fad30ccf16 Mon Sep 17 00:00:00 2001 From: Leo Liu <65527170+CuboiLeo@users.noreply.github.com> Date: Tue, 28 May 2024 21:09:28 -0400 Subject: [PATCH] referee alignment fixed, safe mode when referee package lose connection added --- src/algo/inc/Swerve_Locomotion.h | 2 +- src/algo/src/Swerve_Locomotion.c | 42 ++++++++++++++++++++------------ src/app/inc/launch_task.h | 5 ++++ src/app/src/chassis_task.c | 5 ++-- src/app/src/debug_task.c | 33 ++++++++++++------------- src/app/src/launch_task.c | 8 ++---- src/app/src/robot.c | 2 ++ src/app/src/ui_task.c | 2 +- src/devices/inc/referee_system.h | 4 +-- src/devices/src/referee_system.c | 6 ++++- src/ui/src/ui_indicator_0_11.c | 2 +- 11 files changed, 63 insertions(+), 48 deletions(-) diff --git a/src/algo/inc/Swerve_Locomotion.h b/src/algo/inc/Swerve_Locomotion.h index d4131566..17f72955 100644 --- a/src/algo/inc/Swerve_Locomotion.h +++ b/src/algo/inc/Swerve_Locomotion.h @@ -19,7 +19,7 @@ #define NUMBER_OF_MODULES 4 #define SWERVE_OPTIMIZE -#define POWER_CONTROL +#define POWER_REGULATION typedef struct { diff --git a/src/algo/src/Swerve_Locomotion.c b/src/algo/src/Swerve_Locomotion.c index 97f842d6..924d17e2 100644 --- a/src/algo/src/Swerve_Locomotion.c +++ b/src/algo/src/Swerve_Locomotion.c @@ -261,27 +261,37 @@ void Swerve_Drive(float x, float y, float omega) y *= SWERVE_MAX_SPEED; omega *= SWERVE_MAX_ANGLUAR_SPEED; // convert to rad/s #ifdef POWER_REGULATION - if(fabs(x) > 0.1f || fabs(y) > 0.1f || fabs(omega) > 0.1f) + if(Referee_System.Online_Flag) { - __MOVING_AVERAGE(g_robot_state.chassis_power_buffer,g_robot_state.chassis_power_index, - Referee_Robot_State.Chassis_Power,g_robot_state.chassis_power_count,g_robot_state.chassis_total_power,g_robot_state.chassis_avg_power); - if(g_robot_state.chassis_avg_power < (Referee_Robot_State.Chassis_Power_Max*0.7f)) - g_robot_state.power_increment_ratio += 0.001f; + if(fabs(x) > 0.1f || fabs(y) > 0.1f || fabs(omega) > 0.1f) + { + __MOVING_AVERAGE(g_robot_state.chassis_power_buffer,g_robot_state.chassis_power_index, + Referee_Robot_State.Chassis_Power,g_robot_state.chassis_power_count,g_robot_state.chassis_total_power,g_robot_state.chassis_avg_power); + if(g_robot_state.chassis_avg_power < (Referee_Robot_State.Chassis_Power_Max*0.7f)) + g_robot_state.power_increment_ratio += 0.001f; + else + g_robot_state.power_increment_ratio -= 0.001f; + } else - g_robot_state.power_increment_ratio -= 0.001f; - } + { + memset(g_robot_state.chassis_power_buffer,0,sizeof(g_robot_state.chassis_power_buffer)); + g_robot_state.chassis_power_index = 0; + g_robot_state.chassis_power_count = 0; + g_robot_state.chassis_total_power = 0; + g_robot_state.power_increment_ratio = 1.0f; + } + __MAX_LIMIT(g_robot_state.power_increment_ratio, 0.8f, 5.0f); + x *= g_robot_state.power_increment_ratio; // convert to m/s + y *= g_robot_state.power_increment_ratio; + omega *= g_robot_state.power_increment_ratio; // convert to rad/s + } else { - memset(g_robot_state.chassis_power_buffer,0,sizeof(g_robot_state.chassis_power_buffer)); - g_robot_state.chassis_power_index = 0; - g_robot_state.chassis_power_count = 0; - g_robot_state.chassis_total_power = 0; - g_robot_state.power_increment_ratio = 1.0f; + g_robot_state.power_increment_ratio = 1 + Referee_Robot_State.Manual_Level*0.1f; + x *= g_robot_state.power_increment_ratio; // convert to m/s + y *= g_robot_state.power_increment_ratio; + omega *= g_robot_state.power_increment_ratio; // convert to rad/s } - __MAX_LIMIT(g_robot_state.power_increment_ratio, 0.8f, 5.0f); - x *= g_robot_state.power_increment_ratio; // convert to m/s - y *= g_robot_state.power_increment_ratio; - omega *= g_robot_state.power_increment_ratio; // convert to rad/s #endif Chassis_Speeds_t desired_chassis_speeds = {.x = x, .y = y, .omega = omega}; Set_Desired_States(Chassis_Speeds_To_Module_States(desired_chassis_speeds)); diff --git a/src/app/inc/launch_task.h b/src/app/inc/launch_task.h index 5bfc0289..46dd6309 100644 --- a/src/app/inc/launch_task.h +++ b/src/app/inc/launch_task.h @@ -7,6 +7,8 @@ #define FLYWHEEL_VELOCITY_23 (6000.0f * M3508_REDUCTION_RATIO) #define FLYWHEEL_VELOCITY_30 (7000.0f * M3508_REDUCTION_RATIO) #define FEED_HOLE_NUM (6.0f) +#define LAUNCH_FREQUENCY (12) +#define LAUNCH_PERIOD (1000.0f/LAUNCH_FREQUENCY) #define FEED_1_PROJECTILE_ANGLE (2.0f*PI/FEED_HOLE_NUM) #define FEED_FREQUENCY_6 (6.0f / FEED_HOLE_NUM * 60.0f) #define FEED_FREQUENCY_12 (12.0f / FEED_HOLE_NUM * 60.0f) @@ -22,6 +24,9 @@ typedef struct uint8_t single_launch_finished_flag; uint8_t burst_launch_flag; uint8_t flywheel_enabled; + + uint8_t calculated_heat; + uint8_t referee_update_count; } Launch_Target_t; void Launch_Task_Init(void); diff --git a/src/app/src/chassis_task.c b/src/app/src/chassis_task.c index e9151b66..b91968af 100644 --- a/src/app/src/chassis_task.c +++ b/src/app/src/chassis_task.c @@ -6,7 +6,7 @@ #include "imu_task.h" #include "Swerve_Locomotion.h" -#define SPINTOP_RAMP_COEF (0.003f) +#define SPINTOP_RAMP_COEF (0.03f) #define SPIN_TOP_OMEGA (1.0f) extern Robot_State_t g_robot_state; @@ -23,7 +23,6 @@ void Chassis_Task_Init() void Chassis_Ctrl_Loop() { - if (g_robot_state.enabled) { /* @@ -34,7 +33,7 @@ void Chassis_Ctrl_Loop() if (g_robot_state.spintop_mode) { float translation_speed = sqrtf(powf(g_robot_state.chassis_x_speed, 2) + powf(g_robot_state.chassis_y_speed, 2)); - float spin_coeff = chassis_rad * SPIN_TOP_OMEGA / (translation_speed + chassis_rad * SPIN_TOP_OMEGA); + float spin_coeff = chassis_rad * SPIN_TOP_OMEGA / (translation_speed*0.1f + chassis_rad * SPIN_TOP_OMEGA); // ramp up to target omega float target_omega = SPIN_TOP_OMEGA * spin_coeff; diff --git a/src/app/src/debug_task.c b/src/app/src/debug_task.c index f59c344a..a89e7ba4 100644 --- a/src/app/src/debug_task.c +++ b/src/app/src/debug_task.c @@ -22,7 +22,7 @@ extern uint64_t t; extern Daemon_Instance_t *g_daemon_instances[3]; extern Daemon_Instance_t *g_remote_daemon; extern Daemon_Instance_t *g_referee_daemon_instance_ptr; -#define PRINT_RUNTIME_STATS +//#define PRINT_RUNTIME_STATS #ifdef PRINT_RUNTIME_STATS char g_debug_buffer[1024*2] = {0}; #endif @@ -35,25 +35,24 @@ const char* bottom_border = "/***** End of Info *****/\r\n"; void Debug_Task_Loop(void) { #ifdef DEBUG_ENABLED - // static uint32_t counter = 0; - // #ifdef PRINT_RUNTIME_STATS - // if (counter % 100 == 0) // Print every 100 cycles - // { - // vTaskGetRunTimeStats(g_debug_buffer); - // DEBUG_PRINTF(&huart6, "%s", top_border); - // DEBUG_PRINTF(&huart6, "%s", g_debug_buffer); - // DEBUG_PRINTF(&huart6, "%s", bottom_border); - // } - // #endif - + static uint32_t counter = 0; + #ifdef PRINT_RUNTIME_STATS + if (counter % 100 == 0) // Print every 100 cycles + { + vTaskGetRunTimeStats(g_debug_buffer); + DEBUG_PRINTF(&huart6, "%s", top_border); + DEBUG_PRINTF(&huart6, "%s", g_debug_buffer); + DEBUG_PRINTF(&huart6, "%s", bottom_border); + } + #endif + DEBUG_PRINTF(&huart6, ">time:%.1f\n>ref:%f\n",(float) counter / 1000.0f * DEBUG_PERIOD,Referee_Robot_State.Chassis_Power); // DEBUG_PRINTF(&huart6, ">time:%.1f\n>yaw:%f\n>pitch:%f\n>roll:%f\n", (float) counter / 1000.0f * DEBUG_PERIOD, // g_imu.deg.yaw, g_imu.deg.pitch, g_imu.deg.roll); // DEBUG_PRINTF(&huart6, ">remote_daemon:%d\n", g_remote_daemon->counter); - // counter++; - // if (counter > 0xFFFFFFFF) { - // counter = 0; - // } + counter++; + if (counter > 0xFFFFFFFF) { + counter = 0; + } //DEBUG_PRINTF(&huart6, ">ref:%f\n>act:%f\n",g_motor_feed->velocity_pid->ref,g_motor_feed->stats->current_vel_rpm); - DEBUG_PRINTF(&huart6, ">ref:%d\n",g_referee_daemon_instance_ptr->counter); #endif } \ No newline at end of file diff --git a/src/app/src/launch_task.c b/src/app/src/launch_task.c index 160cf7de..94147cf5 100644 --- a/src/app/src/launch_task.c +++ b/src/app/src/launch_task.c @@ -4,6 +4,7 @@ #include "remote.h" #include "imu_task.h" #include "user_math.h" +#include "referee_system.h" extern Robot_State_t g_robot_state; extern Remote_t g_remote; @@ -12,7 +13,6 @@ DJI_Motor_Handle_t *g_flywheel_left, *g_flywheel_right, *g_motor_feed; Launch_Target_t g_launch_target; void Feed_Angle_Calc(void); -void _launch_set_flywheel_vel_based_on_level(); void Launch_Task_Init() { Motor_Config_t flywheel_left_config = { @@ -72,7 +72,7 @@ void Launch_Task_Init() { void Launch_Ctrl_Loop() { if (g_robot_state.enabled) { if (g_launch_target.flywheel_enabled) { - _launch_set_flywheel_vel_based_on_level(); + g_launch_target.flywheel_velocity = FLYWHEEL_VELOCITY_30; DJI_Motor_Set_Velocity(g_flywheel_left,g_launch_target.flywheel_velocity); DJI_Motor_Set_Velocity(g_flywheel_right,g_launch_target.flywheel_velocity); Feed_Angle_Calc(); @@ -88,10 +88,6 @@ void Launch_Ctrl_Loop() { } } -void _launch_set_flywheel_vel_based_on_level() { - g_launch_target.flywheel_velocity = FLYWHEEL_VELOCITY_30; -} - void Feed_Angle_Calc() { if (g_launch_target.single_launch_flag && !g_launch_target.single_launch_finished_flag) { diff --git a/src/app/src/robot.c b/src/app/src/robot.c index 6c640e55..d15614b9 100644 --- a/src/app/src/robot.c +++ b/src/app/src/robot.c @@ -13,6 +13,7 @@ #include "referee_system.h" #include "buzzer.h" #include "ui.h" +#include "user_math.h" extern DJI_Motor_Handle_t *g_yaw; @@ -177,6 +178,7 @@ void Robot_Cmd_Loop() if (g_remote.keyboard.Z == 1 && g_key_prev.prev_Z == 0) { Referee_Robot_State.Manual_Level++; + __MAX_LIMIT(Referee_Robot_State.Manual_Level,1,10); } g_key_prev.prev_B = g_remote.keyboard.B; g_key_prev.prev_G = g_remote.keyboard.G; diff --git a/src/app/src/ui_task.c b/src/app/src/ui_task.c index c2a6a49b..14c53bc7 100644 --- a/src/app/src/ui_task.c +++ b/src/app/src/ui_task.c @@ -2,7 +2,7 @@ void UI_Task_Loop(void) { - ui_self_id = Referee_Robot_State.ID; + ui_self_id = 3;//Referee_Robot_State.ID; if (!g_robot_state.UI_enabled) { ui_remove_indicator_0(); diff --git a/src/devices/inc/referee_system.h b/src/devices/inc/referee_system.h index 3d9da0dd..c42909c7 100644 --- a/src/devices/inc/referee_system.h +++ b/src/devices/inc/referee_system.h @@ -26,7 +26,7 @@ //#define ROBOT_TYPE_SENTRY //Standard Default Configuration -#define DEFAULT_STANDARD_POWER_MAX 200 +#define DEFAULT_STANDARD_POWER_MAX 45 #define DEFAULT_STANDARD_LAUNCH_SPEED_MAX 30 #define DEFAULT_STANDARD_HEAT_MAX 200 #define DEFAULT_STANDARD_COOLING_RATE 10 @@ -248,7 +248,7 @@ typedef struct uint32_t State; }RFID; - uint16_t Info_Update_Frame; + uint8_t Info_Update_Frame; uint8_t Online_Flag; } Referee_System_t; diff --git a/src/devices/src/referee_system.c b/src/devices/src/referee_system.c index d32a0906..71c92c9c 100644 --- a/src/devices/src/referee_system.c +++ b/src/devices/src/referee_system.c @@ -41,6 +41,7 @@ void Referee_Set_Robot_State(void) Referee_Robot_State.Cooling_Rate = DEFAULT_STANDARD_COOLING_RATE+(Referee_Robot_State.Manual_Level-1)*5; Referee_Robot_State.Heat_Max = DEFAULT_STANDARD_HEAT_MAX+(Referee_Robot_State.Manual_Level-1)*50; Referee_Robot_State.Launch_Speed_Max = DEFAULT_STANDARD_LAUNCH_SPEED_MAX; + Referee_Robot_State.Chassis_Power = (DEFAULT_STANDARD_POWER_MAX-10)+(Referee_Robot_State.Manual_Level-1)*5; Referee_Robot_State.Chassis_Power_Max = DEFAULT_STANDARD_POWER_MAX+(Referee_Robot_State.Manual_Level-1)*5; #endif @@ -70,7 +71,6 @@ void Referee_System_Init(UART_HandleTypeDef *huart) { Referee_System.huart = huart; Referee_Robot_State.Manual_Level = 1; - HAL_UART_Receive_DMA(huart, Referee_System.Buffer, REFEREE_BUFFER_LEN); g_referee_uart_instance_ptr = UART_Register(huart, Referee_System.Buffer, REFEREE_BUFFER_LEN, Referee_Get_Data); @@ -91,6 +91,10 @@ void Referee_Get_Data(UART_Instance_t *uart_instance) { Daemon_Reload(g_referee_daemon_instance_ptr); Referee_System.Online_Flag = 1; + Referee_System.Info_Update_Frame++; + if(Referee_System.Info_Update_Frame > 100) + Referee_System.Info_Update_Frame = 0; + switch (Referee_System.Buffer[n + 5] | Referee_System.Buffer[n + 6] << 8) { case REFEREE_GAME_STATUS: diff --git a/src/ui/src/ui_indicator_0_11.c b/src/ui/src/ui_indicator_0_11.c index 09b1d19a..a65c2a47 100644 --- a/src/ui/src/ui_indicator_0_11.c +++ b/src/ui/src/ui_indicator_0_11.c @@ -7,7 +7,7 @@ #define FRAME_ID 1 #define GROUP_ID 0 -#define START_ID 11 +#define START_ID 12 ui_string_frame_t ui_indicator_0_11;