From 0e1327e34137ede5a11658b842c1c6cb2f64bb26 Mon Sep 17 00:00:00 2001 From: jia-xie <103150184+jia-xie@users.noreply.github.com> Date: Tue, 11 Jun 2024 23:47:46 -0400 Subject: [PATCH] RMUL version (#62) * referee uart bug sometimes occur, level indicator added * referee alignment fixed, safe mode when referee package lose connection added * barrel heat control added, not fully tested * barrel heat regulation added, still need some final adjustments * huge bug found, gonna fix it tomorrow * bug fixed * power regulation tuned * final final tuning -leo --------- Co-authored-by: Leo Liu <65527170+CuboiLeo@users.noreply.github.com> --- Makefile | 2 + src/algo/inc/Swerve_Locomotion.h | 2 +- src/algo/inc/user_math.h | 2 +- src/algo/src/Swerve_Locomotion.c | 99 +++++++++----------- src/app/inc/launch_task.h | 12 ++- src/app/inc/robot.h | 4 +- src/app/inc/robot_tasks.h | 4 +- src/app/src/chassis_task.c | 5 +- src/app/src/debug_task.c | 43 +++++---- src/app/src/gimbal_task.c | 4 +- src/app/src/launch_task.c | 80 ++++++++++------ src/app/src/motor_task.c | 15 ++- src/app/src/robot.c | 19 +++- src/app/src/ui_task.c | 107 +++++++++++---------- src/devices/inc/dji_motor.h | 1 + src/devices/inc/motor.h | 2 +- src/devices/inc/referee_system.h | 91 +++++------------- src/devices/inc/supercap.h | 22 +++++ src/devices/src/dji_motor.c | 20 +++- src/devices/src/referee_system.c | 61 +++++++++--- src/devices/src/supercap.c | 27 ++++++ src/ui/inc/ui.h | 153 ++++++++++++++++--------------- src/ui/inc/ui_indicator_0_11.h | 16 ++++ src/ui/inc/ui_indicator_1_0.h | 1 + src/ui/inc/ui_interface.h | 4 +- src/ui/src/ui_indicator_0_11.c | 47 ++++++++++ src/ui/src/ui_indicator_1_0.c | 12 ++- 27 files changed, 521 insertions(+), 334 deletions(-) create mode 100644 src/devices/inc/supercap.h create mode 100644 src/devices/src/supercap.c create mode 100644 src/ui/inc/ui_indicator_0_11.h create mode 100644 src/ui/src/ui_indicator_0_11.c diff --git a/Makefile b/Makefile index 0714899a..23a48941 100644 --- a/Makefile +++ b/Makefile @@ -135,6 +135,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/supercap.c \ src/app/src/motor_task.c \ src/app/src/chassis_task.c \ src/app/src/gimbal_task.c \ @@ -154,6 +155,7 @@ src/ui/src/ui_indicator_0_7.c \ src/ui/src/ui_indicator_0_8.c \ src/ui/src/ui_indicator_0_9.c \ src/ui/src/ui_indicator_0_10.c \ +src/ui/src/ui_indicator_0_11.c \ src/ui/src/ui_indicator_1_0.c # ASM sources 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/inc/user_math.h b/src/algo/inc/user_math.h index b01cf453..6ffd307c 100644 --- a/src/algo/inc/user_math.h +++ b/src/algo/inc/user_math.h @@ -39,7 +39,7 @@ } \ } while (0); -#define BUFFER_SIZE 500 +#define BUFFER_SIZE (1000) #define __MOVING_AVERAGE(buffer, index, update_value, count, sum, average) \ do \ { \ diff --git a/src/algo/src/Swerve_Locomotion.c b/src/algo/src/Swerve_Locomotion.c index 97f842d6..5c2b62a0 100644 --- a/src/algo/src/Swerve_Locomotion.c +++ b/src/algo/src/Swerve_Locomotion.c @@ -6,8 +6,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}; +Module_State_t optimized_module_state; float last_swerve_angle[NUMBER_OF_MODULES] = {0.0f, 0.0f, 0.0f, 0.0f}; -Kalman_Filter_t power_KF = {.Prev_P = 1.0f, .Q = 0.0001, .R = 5.0f}; /** * @brief Inverse kinematics matrix for a 4 module swerve, defined counterclockwise from the front left @@ -42,14 +42,14 @@ void Set_Module_Output(Swerve_Module_t *swerve_module, Module_State_t desired_st void Swerve_Init() { // define constants for each module in an array [0] == fl, [1] == rl, [2] == rr, [3] == fr - int azimuth_can_bus_array[NUMBER_OF_MODULES] = {1, 1, 2, 2}; + int azimuth_can_bus_array[NUMBER_OF_MODULES] = {2, 2, 2, 2}; int azimuth_speed_controller_id_array[NUMBER_OF_MODULES] = {1, 2, 3, 4}; - int azimuth_offset_array[NUMBER_OF_MODULES] = {1990, 730, 6060, 1362}; + int azimuth_offset_array[NUMBER_OF_MODULES] = {2050, 1940, 1430, 8150}; Motor_Reversal_t azimuth_motor_reversal_array[NUMBER_OF_MODULES] = {MOTOR_REVERSAL_REVERSED, MOTOR_REVERSAL_REVERSED, MOTOR_REVERSAL_REVERSED, MOTOR_REVERSAL_REVERSED}; - int drive_can_bus_array[NUMBER_OF_MODULES] = {1, 1, 2, 2}; + int drive_can_bus_array[NUMBER_OF_MODULES] = {1, 2, 2, 2}; int drive_speed_controller_id_array[NUMBER_OF_MODULES] = {1, 2, 3, 4}; - Motor_Reversal_t drive_motor_reversal_array[NUMBER_OF_MODULES] = {MOTOR_REVERSAL_REVERSED, MOTOR_REVERSAL_REVERSED, MOTOR_REVERSAL_NORMAL, MOTOR_REVERSAL_NORMAL}; + Motor_Reversal_t drive_motor_reversal_array[NUMBER_OF_MODULES] = {MOTOR_REVERSAL_NORMAL, MOTOR_REVERSAL_NORMAL, MOTOR_REVERSAL_REVERSED, MOTOR_REVERSAL_REVERSED}; // init common PID configuration for azimuth motors Motor_Config_t azimuth_motor_config = { @@ -179,45 +179,22 @@ Module_State_t Optimize_Module_Angle(Module_State_t input_state, float measured_ { Module_State_t optimized_module_state = {0}; optimized_module_state.speed = input_state.speed; - float target_angle = fmodf(input_state.angle, 2.0f * PI); // get positive normalized target angle - if (target_angle < 0) - { - target_angle += 2.0f * PI; - } + optimized_module_state.angle = input_state.angle; - float curr_angle_normalized = fmodf(measured_angle, 2 * PI); // get normalized current angle - if (curr_angle_normalized < 0) - { - curr_angle_normalized += 2.0f * PI; - } + if(measured_angle > PI) + measured_angle -= 2*PI; - float delta = target_angle - curr_angle_normalized; // calculate the angle delta + float angle_diff = optimized_module_state.angle - measured_angle; - // these conditions essentially create the step function for angles to "snap" too offset from the measured angle to avoid flipping - if (PI / 2.0f < fabsf(delta) && fabsf(delta) < 3 * PI / 2.0f) - { - float beta = PI - fabsf(delta); - beta *= (delta > 0 ? 1.0f : -1.0f); - target_angle = measured_angle - beta; - optimized_module_state.speed = -1.0f * input_state.speed; // invert velocity if optimal angle 180 deg from target - } - else if (fabsf(delta) >= 3.0f * PI / 2.0f) - { - if (delta < 0) - { - target_angle = measured_angle + (2.0f * PI + delta); - } - else - { - target_angle = measured_angle - (2.0f * PI - delta); - } - } - else - { - target_angle = measured_angle + delta; - } + if((angle_diff >= PI/2 && angle_diff <= 3*PI/2) || (angle_diff <= -PI/2 && angle_diff >= -3*PI/2)) + { + optimized_module_state.angle += (angle_diff > 0) ? -PI:PI; + optimized_module_state.speed *= -1.0f; + } + + if (optimized_module_state.angle < 0) + optimized_module_state.angle += 2.0f * PI; - optimized_module_state.angle = target_angle; return optimized_module_state; } @@ -261,27 +238,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.9f-Referee_Robot_State.Level*0.2f))) + 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..84c5db83 100644 --- a/src/app/inc/launch_task.h +++ b/src/app/inc/launch_task.h @@ -4,15 +4,19 @@ #include #include "dji_motor.h" -#define FLYWHEEL_VELOCITY_23 (6000.0f * M3508_REDUCTION_RATIO) +#define FLYWHEEL_VELOCITY_10 (3000.0f * M3508_REDUCTION_RATIO) #define FLYWHEEL_VELOCITY_30 (7000.0f * M3508_REDUCTION_RATIO) #define FEED_HOLE_NUM (6.0f) +#define LAUNCH_FREQUENCY (20) +#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) -#define FEED_FREQUENCY_18 (18.0f / FEED_HOLE_NUM * 60.0f) +#define FEED_FREQUENCY_20 (20.0f / FEED_HOLE_NUM * 60.0f) #define FEED_FREQUENCY_30 (30.0f / FEED_HOLE_NUM * 60.0f) +//#define HEAT_CONTROL + typedef struct { float flywheel_velocity; @@ -22,6 +26,10 @@ typedef struct uint8_t single_launch_finished_flag; uint8_t burst_launch_flag; uint8_t flywheel_enabled; + + int16_t calculated_heat; + uint16_t heat_count; + uint16_t launch_freq_count; } Launch_Target_t; void Launch_Task_Init(void); diff --git a/src/app/inc/robot.h b/src/app/inc/robot.h index 8efad3b5..00c60599 100644 --- a/src/app/inc/robot.h +++ b/src/app/inc/robot.h @@ -2,6 +2,7 @@ #define ROBOT_H #include +#include "user_math.h" typedef struct { @@ -14,7 +15,7 @@ typedef struct float chassis_y_speed; float chassis_omega; - float chassis_power_buffer[500]; + float chassis_power_buffer[BUFFER_SIZE]; uint16_t chassis_power_index; uint16_t chassis_power_count; float chassis_avg_power; @@ -34,6 +35,7 @@ typedef struct uint8_t prev_B; uint8_t prev_G; uint8_t prev_V; + uint8_t prev_Z; uint8_t prev_left_switch; } Key_Prev_t; diff --git a/src/app/inc/robot_tasks.h b/src/app/inc/robot_tasks.h index c778ea51..84942458 100644 --- a/src/app/inc/robot_tasks.h +++ b/src/app/inc/robot_tasks.h @@ -89,7 +89,7 @@ void Robot_Tasks_UI(void const *argument) { portTickType xLastWakeTime; xLastWakeTime = xTaskGetTickCount(); - const TickType_t TimeIncrement = pdMS_TO_TICKS(100); + const TickType_t TimeIncrement = pdMS_TO_TICKS(1); while (1) { UI_Task_Loop(); @@ -116,7 +116,7 @@ void Robot_Tasks_Jetson_Orin(void const *argument) const TickType_t TimeIncrement = pdMS_TO_TICKS(JETSON_ORIN_PERIOD); while (1) { - //Jetson_Orin_Send_Data(); + Jetson_Orin_Send_Data(); vTaskDelayUntil(&xLastWakeTime, TimeIncrement); } } diff --git a/src/app/src/chassis_task.c b/src/app/src/chassis_task.c index e9151b66..409cd254 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.2f + 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 a4f1c5be..f39ee068 100644 --- a/src/app/src/debug_task.c +++ b/src/app/src/debug_task.c @@ -15,13 +15,14 @@ extern Robot_State_t g_robot_state; extern DJI_Motor_Handle_t *g_yaw; extern IMU_t g_imu; -extern Swerve_Module_t g_swerve_fl; +extern Swerve_Module_t g_swerve_rl, g_swerve_fr; extern Remote_t g_remote; extern Launch_Target_t g_launch_target; -extern uint64_t t; extern Daemon_Instance_t *g_daemon_instances[3]; extern Daemon_Instance_t *g_remote_daemon; -#define PRINT_RUNTIME_STATS +extern Daemon_Instance_t *g_referee_daemon_instance_ptr; +extern float test_tmd; +//#define PRINT_RUNTIME_STATS #ifdef PRINT_RUNTIME_STATS char g_debug_buffer[1024*2] = {0}; #endif @@ -29,29 +30,31 @@ char g_debug_buffer[1024*2] = {0}; const char* top_border = "\r\n\r\n\r\n/***** System Info *****/\r\n"; const char* bottom_border = "/***** End of Info *****/\r\n"; -#define DEBUG_ENABLED +//#define DEBUG_ENABLED 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; - // } - DEBUG_PRINTF(&huart6, ">ref:%f\n>act:%f\n",g_motor_feed->velocity_pid->ref,g_motor_feed->stats->current_vel_rpm); + counter++; + if (counter > 0xFFFFFFFF) { + counter = 0; + } + +// DEBUG_PRINTF(&huart1, ">fr_ref_speed:%f\n>fr_actual_speed:%f\n", +// g_swerve_fr.drive_motor->velocity_pid->ref,g_swerve_fr.drive_motor->stats->current_vel_rpm); #endif } \ No newline at end of file diff --git a/src/app/src/gimbal_task.c b/src/app/src/gimbal_task.c index 4a02998d..ac053345 100644 --- a/src/app/src/gimbal_task.c +++ b/src/app/src/gimbal_task.c @@ -42,7 +42,7 @@ void Gimbal_Task_Init() Motor_Config_t yaw_motor_config = { .can_bus = 1, .speed_controller_id = 3, - .offset = 3690, + .offset = 2400, .control_mode = POSITION_VELOCITY_SERIES, .motor_reversal = MOTOR_REVERSAL_NORMAL, .use_external_feedback = 1, @@ -67,7 +67,7 @@ void Gimbal_Task_Init() }; Motor_Config_t pitch_motor_config = { - .can_bus = 2, + .can_bus = 1, .speed_controller_id = 2, .offset = 4460, .use_external_feedback = 1, diff --git a/src/app/src/launch_task.c b/src/app/src/launch_task.c index 160cf7de..a85430e8 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,12 +13,11 @@ 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 = { - .can_bus = 2, - .speed_controller_id = 1, + .can_bus = 1, + .speed_controller_id = 4, .offset = 0, .control_mode = VELOCITY_CONTROL, .motor_reversal = MOTOR_REVERSAL_REVERSED, @@ -29,8 +29,8 @@ void Launch_Task_Init() { }; Motor_Config_t flywheel_right_config = { - .can_bus = 2, - .speed_controller_id = 2, + .can_bus = 1, + .speed_controller_id = 5, .offset = 0, .control_mode = VELOCITY_CONTROL, .motor_reversal = MOTOR_REVERSAL_NORMAL, @@ -43,7 +43,7 @@ void Launch_Task_Init() { Motor_Config_t feed_speed_config = { .can_bus = 1, - .speed_controller_id = 3, + .speed_controller_id = 2, .offset = 0, .control_mode = VELOCITY_CONTROL | POSITION_CONTROL, .motor_reversal = MOTOR_REVERSAL_NORMAL, @@ -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,27 +88,51 @@ 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) { - g_launch_target.feed_angle = DJI_Motor_Get_Total_Angle(g_motor_feed) + FEED_1_PROJECTILE_ANGLE; - g_launch_target.single_launch_finished_flag = 1; - } - else if (g_launch_target.single_launch_flag && g_launch_target.single_launch_finished_flag) { - DJI_Motor_Set_Control_Mode(g_motor_feed, POSITION_CONTROL_TOTAL_ANGLE); - DJI_Motor_Set_Angle(g_motor_feed,g_launch_target.feed_angle); - } - else if (g_launch_target.burst_launch_flag) { - g_launch_target.feed_velocity = FEED_FREQUENCY_12; - DJI_Motor_Set_Control_Mode(g_motor_feed, VELOCITY_CONTROL); - DJI_Motor_Set_Velocity(g_motor_feed,g_launch_target.feed_velocity); - } - else { - DJI_Motor_Set_Control_Mode(g_motor_feed, VELOCITY_CONTROL); - DJI_Motor_Set_Velocity(g_motor_feed,0); - } + #ifdef HEAT_CONTROL + if (Referee_System.Online_Flag) + { + if (g_launch_target.heat_count*2 % 100 == 0) + { + g_launch_target.calculated_heat -= Referee_Robot_State.Cooling_Rate/10; + __MAX_LIMIT(g_launch_target.calculated_heat,0,Referee_Robot_State.Heat_Max); + } + if (g_launch_target.burst_launch_flag) + { + if (g_launch_target.launch_freq_count*2 > LAUNCH_PERIOD) + { + g_launch_target.launch_freq_count = 0; + if((Referee_Robot_State.Heat_Max-g_launch_target.calculated_heat) > 20) + { + g_launch_target.calculated_heat += 10; + g_launch_target.feed_angle += FEED_1_PROJECTILE_ANGLE; + + } + } + DJI_Motor_Set_Control_Mode(g_motor_feed, POSITION_CONTROL_TOTAL_ANGLE); + DJI_Motor_Set_Angle(g_motor_feed,g_launch_target.feed_angle); + } + } + #else + if (g_launch_target.single_launch_flag && !g_launch_target.single_launch_finished_flag) { + g_launch_target.feed_angle = DJI_Motor_Get_Total_Angle(g_motor_feed) + FEED_1_PROJECTILE_ANGLE; + g_launch_target.single_launch_finished_flag = 1; + } + else if (g_launch_target.single_launch_flag && g_launch_target.single_launch_finished_flag) { + DJI_Motor_Set_Control_Mode(g_motor_feed, POSITION_CONTROL_TOTAL_ANGLE); + DJI_Motor_Set_Angle(g_motor_feed,g_launch_target.feed_angle); + } + else if (g_launch_target.burst_launch_flag) { + g_launch_target.feed_velocity = FEED_FREQUENCY_20; + DJI_Motor_Set_Control_Mode(g_motor_feed, VELOCITY_CONTROL); + DJI_Motor_Set_Velocity(g_motor_feed,g_launch_target.feed_velocity); + } + else { + DJI_Motor_Set_Control_Mode(g_motor_feed, VELOCITY_CONTROL); + DJI_Motor_Set_Velocity(g_motor_feed,0); + } + #endif + + } diff --git a/src/app/src/motor_task.c b/src/app/src/motor_task.c index 760cf129..58119ed6 100644 --- a/src/app/src/motor_task.c +++ b/src/app/src/motor_task.c @@ -2,10 +2,19 @@ #include "dji_motor.h" #include "dm_motor.h" #include "mf_motor.h" +#include "supercap.h" + +extern Supercap_t g_supercap; void Motor_Task_Loop() { - DJI_Motor_Send(); - MF_Motor_Send(); - DM_Motor_Send(); + // DJI_Motor_Send(); + // MF_Motor_Send(); + // DM_Motor_Send(); + + g_supercap.send_counter++; + if (g_supercap.send_counter >= 100) { + Supercap_Send(); + g_supercap.send_counter = 0; + } } diff --git a/src/app/src/robot.c b/src/app/src/robot.c index 69587ae0..54dc263f 100644 --- a/src/app/src/robot.c +++ b/src/app/src/robot.c @@ -13,6 +13,9 @@ #include "referee_system.h" #include "buzzer.h" #include "ui.h" +#include "user_math.h" +#include "Swerve_Locomotion.h" +#include "supercap.h" extern DJI_Motor_Handle_t *g_yaw; @@ -24,6 +27,7 @@ Robot_State_t g_robot_state = {0, 0}; Key_Prev_t g_key_prev = {0}; extern Launch_Target_t g_launch_target; extern Remote_t g_remote; +extern Supercap_t g_supercap; uint8_t g_start_safely = 0; @@ -47,7 +51,8 @@ void Robot_Init() Remote_Init(&huart3); CAN_Service_Init(); Referee_System_Init(&huart1); - // Jetson_Orin_Init(&huart6); + Jetson_Orin_Init(&huart6); + Supercap_Init(&g_supercap); // Initialize all tasks Robot_Tasks_Start(); } @@ -134,7 +139,8 @@ void Robot_Cmd_Loop() /* Gimbal ends here */ /* Launch control starts here */ - if (Referee_Robot_State.Shooter_Heat_1 < (Referee_Robot_State.Heat_Max-40)) + g_launch_target.heat_count++; + if (1)//Referee_Robot_State.Shooter_Heat_1 < (Referee_Robot_State.Heat_Max-10)) { if (g_remote.controller.wheel < -50.0f) { // dial wheel forward single fire @@ -145,12 +151,15 @@ void Robot_Cmd_Loop() { // dial wheel backward burst fire g_launch_target.single_launch_flag = 0; g_launch_target.burst_launch_flag = 1; + g_launch_target.launch_freq_count++; } else { // dial wheel mid stop fire g_launch_target.single_launch_flag = 0; g_launch_target.single_launch_finished_flag = 0; g_launch_target.burst_launch_flag = 0; + g_launch_target.launch_freq_count = 0; + g_launch_target.feed_angle = DJI_Motor_Get_Total_Angle(g_motor_feed); } } else @@ -174,9 +183,15 @@ void Robot_Cmd_Loop() { _toggle_robot_state(&g_robot_state.UI_enabled); } + 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; g_key_prev.prev_V = g_remote.keyboard.V; + g_key_prev.prev_Z = g_remote.keyboard.Z; /* Keyboard Toggles Start Here */ /* AutoAiming Flag, not used only for debug */ diff --git a/src/app/src/ui_task.c b/src/app/src/ui_task.c index bbaaf719..c2a6a49b 100644 --- a/src/app/src/ui_task.c +++ b/src/app/src/ui_task.c @@ -2,54 +2,61 @@ void UI_Task_Loop(void) { - ui_self_id = Referee_Robot_State.ID; - if (!g_robot_state.UI_enabled) - { - ui_remove_indicator_0(); - ui_init_indicator_0(); - ui_remove_indicator_1(); - ui_init_indicator_1(); - g_robot_state.UI_enabled = 1; - } - if (g_launch_target.flywheel_enabled) - { - ui_indicator_1_Flywheel_Select->start_x = 270; - ui_indicator_1_Flywheel_Select->end_x = 320; - } - else - { - ui_indicator_1_Flywheel_Select->start_x = 335; - ui_indicator_1_Flywheel_Select->end_x = 385; - } - if (g_robot_state.spintop_mode) - { - ui_indicator_1_Spintop_Select->start_x = 270; - ui_indicator_1_Spintop_Select->end_x = 320; - } - else - { - ui_indicator_1_Spintop_Select->start_x = 335; - ui_indicator_1_Spintop_Select->end_x = 385; - } - if (g_robot_state.autoaiming_enabled) - { - ui_indicator_1_Autoaim_Select->start_x = 270; - ui_indicator_1_Autoaim_Select->end_x = 320; - ui_indicator_1_Aim_H_Line->color = 2; - ui_indicator_1_Aim_V_Line->color = 2; - } - else - { - ui_indicator_1_Autoaim_Select->start_x = 335; - ui_indicator_1_Autoaim_Select->end_x = 385; - ui_indicator_1_Aim_H_Line->color = 3; - ui_indicator_1_Aim_V_Line->color = 3; - } - if (ui_indicator_1_Supercap->number>=99) - { - ui_indicator_1_Supercap->number = 0; - } - ui_indicator_1_Supercap->number++; - - ui_update_indicator_1(); + ui_self_id = Referee_Robot_State.ID; + if (!g_robot_state.UI_enabled) + { + ui_remove_indicator_0(); + ui_init_indicator_0(); + ui_remove_indicator_1(); + ui_init_indicator_1(); + g_robot_state.UI_enabled = 1; + } + if (g_launch_target.flywheel_enabled) + { + ui_indicator_1_Flywheel_Select->start_x = 270; + ui_indicator_1_Flywheel_Select->end_x = 320; + } + else + { + ui_indicator_1_Flywheel_Select->start_x = 335; + ui_indicator_1_Flywheel_Select->end_x = 385; + } + if (g_robot_state.spintop_mode) + { + ui_indicator_1_Spintop_Select->start_x = 270; + ui_indicator_1_Spintop_Select->end_x = 320; + } + else + { + ui_indicator_1_Spintop_Select->start_x = 335; + ui_indicator_1_Spintop_Select->end_x = 385; + } + if (g_robot_state.autoaiming_enabled) + { + ui_indicator_1_Autoaim_Select->start_x = 270; + ui_indicator_1_Autoaim_Select->end_x = 320; + ui_indicator_1_Aim_H_Line->color = 2; + ui_indicator_1_Aim_V_Line->color = 2; + } + else + { + ui_indicator_1_Autoaim_Select->start_x = 335; + ui_indicator_1_Autoaim_Select->end_x = 385; + ui_indicator_1_Aim_H_Line->color = 3; + ui_indicator_1_Aim_V_Line->color = 3; + } + if (ui_indicator_1_Supercap->number>=99) + { + ui_indicator_1_Supercap->number = 0; + } + if(Referee_System.Online_Flag) + { + ui_indicator_1_Level_Indicator->number = Referee_Robot_State.Level; + } + else + { + ui_indicator_1_Level_Indicator->number = Referee_Robot_State.Manual_Level; + } + ui_indicator_1_Supercap->number++; + ui_update_indicator_1(); } \ No newline at end of file diff --git a/src/devices/inc/dji_motor.h b/src/devices/inc/dji_motor.h index 91df0510..0466136b 100644 --- a/src/devices/inc/dji_motor.h +++ b/src/devices/inc/dji_motor.h @@ -39,6 +39,7 @@ typedef struct DJI_Motor_Stats_s { uint16_t encoder_offset; int32_t total_round; float absolute_angle_rad; + float last_absolute_angle_rad; float total_angle_rad; float reduction_ratio; } DJI_Motor_Stats_t; diff --git a/src/devices/inc/motor.h b/src/devices/inc/motor.h index 64597edb..b13369f7 100644 --- a/src/devices/inc/motor.h +++ b/src/devices/inc/motor.h @@ -20,7 +20,7 @@ typedef enum Motor_Reversal_e typedef struct { uint8_t can_bus; - uint16_t tx_id; // ignore this for + uint16_t tx_id; // ignore this for DJI motors uint16_t rx_id; // use can_id for general devices uint8_t speed_controller_id; // use speed_contrller_id for dji motors uint16_t offset; diff --git a/src/devices/inc/referee_system.h b/src/devices/inc/referee_system.h index 3a4c7099..c42909c7 100644 --- a/src/devices/inc/referee_system.h +++ b/src/devices/inc/referee_system.h @@ -21,75 +21,27 @@ #define REFEREE_TIMEOUT_MS 500 -//Standard Confrontation -#define V1_STANDARD_POWER_MAX 120 -#define V1_STANDARD_HP_MAX 200 -#define V1_STANDARD_LAUNCH_SPEED_MAX 30 -#define V1_STANDARD_HEAT_MAX 280 -#define V1_STANDARD_COOLING_RATE 25 +#define ROBOT_TYPE_STANDARD +//#define ROBOT_TYPE_HERO +//#define ROBOT_TYPE_SENTRY -#define V1_SBR_POWER_MAX 150 -#define V1_SBR_HP_MAX 200 -#define V1_SBR_LAUNCH_SPEED_MAX 30 -#define V1_SBR_HEAT_MAX 280 -#define V1_SBR_COOLING_RATE 50 +//Standard Default Configuration +#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 -//Sentry Configuration -#define SENTRY_POWER_MAX 100 -#define SENTRY_HP_MAX 600 -#define SENTRY_LAUNCH_SPEED_MAX 30 -#define SENTRY_HEAT_MAX 400 -#define SENTRY_COOLING_RATE 80 +//Sentry Default Configuration +#define DEFAULT_SENTRY_POWER_MAX 100 +#define DEFAULT_SENTRY_LAUNCH_SPEED_MAX 30 +#define DEFAULT_SENTRY_HEAT_MAX 400 +#define DEFAULT_SENTRY_COOLING_RATE 80 -//3V3 Confrontation -#define V3_STANDARD_LAUNCH_SPEED_MAX 30 -#define V3_STANDARD_LV10_HP_MAX 400 -#define V3_STANDARD_LV10_POWER_MAX 100 - -#define V3_HERO_LAUNCH_SPEED_MAX 16 -#define V3_HERO_LV10_HP_MAX 500 -#define V3_HERO_LV10_POWER_MAX 120 - -#define V3_HERO_POWER_FOCUSED_LV1_HP_MAX 200 -#define V3_HERO_POWER_FOCUSED_HP_INCREMENT 25 -#define V3_HERO_POWER_FOCUSED_LV1_POWER_MAX 70 -#define V3_HERO_POWER_FOCUSED_POWER_INCREMENT 5 - -#define V3_HERO_HP_FOCUSED_LV1_HP_MAX 250 -#define V3_HERO_HP_FOCUSED_HP_INCREMENT 25 -#define V3_HERO_HP_FOCUSED_LV1_POWER_MAX 55 -#define V3_HERO_HP_FOCUSED_POWER_INCREMENT 5 -#define V3_HERO_HP_FOCUSED_LV9_POWER_MAX 100 - -#define V3_STANDARD_POWER_FOCUSED_LV1_HP_MAX 150 -#define V3_STANDARD_POWER_FOCUSED_HP_INCREMENT 25 -#define V3_STANDARD_POWER_FOCUSED_LV1_POWER_MAX 60 -#define V3_STANDARD_POWER_FOCUSED_POWER_INCREMENT 5 - -#define V3_STANDARD_HP_FOCUSED_LV1_HP_MAX 200 -#define V3_STANDARD_HP_FOCUSED_HP_INCREMENT 25 -#define V3_STANDARD_HP_FOCUSED_LV1_POWER_MAX 45 -#define V3_STANDARD_HP_FOCUSED_POWER_INCREMENT 5 -#define V3_STANDARD_HP_FOCUSED_LV9_POWER_MAX 90 - -#define V3_STANDARD_BURST_FOCUSED_LV1_HEAT_MAX 200 -#define V3_STANDARD_BURST_FOCUSED_HEAT_INCREMENT 50 -#define V3_STANDARD_BURST_FOCUSED_LV1_COOLING_MAX 10 -#define V3_STANDARD_BURST_FOCUSED_COOLING_INCREMENT 5 - -#define V3_STANDARD_COOLING_FOCUSED_LV1_HEAT_MAX 50 -#define V3_STANDARD_COOLING_FOCUSED_HEAT_INCREMENT 35 -#define V3_STANDARD_COOLING_FOCUSED_LV10_HEAT_MAX 400 -#define V3_STANDARD_COOLING_FOCUSED_LV1_COOLING_MAX 40 -#define V3_STANDARD_COOLING_FOCUSED_COOLING_INCREMENT 5 -#define V3_STANDARD_COOLING_FOCUSED_LV10_COOLING_MAX 80 - -#define V3_HERO_LV1_HEAT_MAX 200 -#define V3_HERO_LV10_HEAT_MAX 200 -#define V3_HERO_HEAT_INCREMENT 30 -#define V3_HERO_LV1_COOLING_MAX 40 -#define V3_HERO_COOLING_INCREMENT 8 -#define V3_HERO_LV10_COOLING_MAX 120 +//Hero Default Configuration +#define DEFAULT_HERO_POWER_MAX 55 +#define DEFAULT_HERO_LAUNCH_SPEED_MAX 16 +#define DEFAULT_HERO_HEAT_MAX 200 +#define DEFAULT_HERO_COOLING_RATE 40 #define REFEREE_BUFFER_LEN 273u //Buffer length to receive all data #define REFEREE_FRAME_HEADER_START 0xA5 //Frame header @@ -142,6 +94,7 @@ typedef struct uint8_t Game_Type; //1 for 7v7, 4 for 3v3, 5 for 1v1 uint8_t ID; //3,4,5 Red Standard - 103,104,105 Blue Standard uint8_t Level; + uint8_t Manual_Level; uint16_t Cooling_Rate; uint16_t Heat_Max; @@ -151,7 +104,7 @@ typedef struct float Chassis_Power; float Power_Buffer; uint16_t Shooter_Heat_1; - uint16_t Shooter_Heat_2; + uint16_t Shooter_Heat_2; uint8_t Shooting_Frequency; float Shooting_Speed; }Referee_Robot_State_t; @@ -295,8 +248,8 @@ typedef struct uint32_t State; }RFID; - uint16_t Info_Update_Frame; - uint8_t Offline_Flag; + uint8_t Info_Update_Frame; + uint8_t Online_Flag; } Referee_System_t; void Referee_Get_Data(UART_Instance_t *uart_instance); diff --git a/src/devices/inc/supercap.h b/src/devices/inc/supercap.h new file mode 100644 index 00000000..7db43b27 --- /dev/null +++ b/src/devices/inc/supercap.h @@ -0,0 +1,22 @@ +#ifndef __SUPERCAP_H +#define __SUPERCAP_H + +#include +#include "bsp_can.h" +#include "referee_system.h" + +typedef struct +{ + uint8_t can_bus; + uint16_t tx_id; + uint16_t rx_id; + uint8_t send_counter; + + uint8_t supercap_percent; +} Supercap_t; + +void Supercap_Init(Supercap_t *g_supercap); +void Supercap_Decode(CAN_Instance_t *can_instance); +void Supercap_Send(void); + +#endif // __SUPERCAP_H \ No newline at end of file diff --git a/src/devices/src/dji_motor.c b/src/devices/src/dji_motor.c index b983fe0a..4f3cb19b 100644 --- a/src/devices/src/dji_motor.c +++ b/src/devices/src/dji_motor.c @@ -7,7 +7,7 @@ DJI_Motor_Handle_t *g_dji_motors[MAX_DJI_MOTORS] = {NULL}; uint8_t g_dji_motor_count = 0; - +float test_tmd; DJI_Send_Group_t *g_dji_send_group[MAX_DJI_MOTOR_GROUPS] = {NULL}; uint8_t g_dji_motor_group_count = 0; @@ -195,7 +195,7 @@ float DJI_Motor_Get_Absolute_Angle(DJI_Motor_Handle_t *motor_handle) return motor_handle->stats->absolute_angle_rad; break; case MOTOR_REVERSAL_REVERSED: - return -motor_handle->stats->absolute_angle_rad; + return -motor_handle->stats->absolute_angle_rad + 2*PI; break; } return -1; @@ -239,7 +239,7 @@ void DJI_Motor_Set_Angle(DJI_Motor_Handle_t *motor_handle, float angle) motor_handle->angle_pid->ref = angle; break; case MOTOR_REVERSAL_REVERSED: - motor_handle->angle_pid->ref = -angle; + motor_handle->angle_pid->ref = -angle+2*PI; break; default: break; @@ -435,18 +435,28 @@ void DJI_Motor_Decode(CAN_Instance_t *can_instance) motor->current_torq = (int16_t)(data[4] << 8 | data[5]); motor->temp = data[6]; + motor->last_absolute_angle_rad = motor->absolute_angle_rad; motor->absolute_angle_rad = motor->current_tick - motor->encoder_offset; + if(motor->absolute_angle_rad >= 8192) + { + motor->absolute_angle_rad -= 8192; + } + else if(motor->absolute_angle_rad < 0) + { + motor->absolute_angle_rad += 8192; + } /* absolute angle */ __MAP(motor->absolute_angle_rad, 0, 8192, 0, 2 * PI); /* angle wrap */ - if (motor->current_tick - motor->last_tick > DJI_HALF_MAX_TICKS) + if (motor->absolute_angle_rad - motor->last_absolute_angle_rad > PI) { motor->total_round--; } - else if (motor->current_tick - motor->last_tick < -4096) + else if (motor->absolute_angle_rad - motor->last_absolute_angle_rad < -PI) { motor->total_round++; } + #pragma message "there are some problem with total_angle_rad" motor->total_angle_rad = ((motor->total_round) * 2 * PI + motor->absolute_angle_rad) * motor->reduction_ratio; } \ No newline at end of file diff --git a/src/devices/src/referee_system.c b/src/devices/src/referee_system.c index cfb834b1..71c92c9c 100644 --- a/src/devices/src/referee_system.c +++ b/src/devices/src/referee_system.c @@ -17,32 +17,60 @@ Daemon_Instance_t *g_referee_daemon_instance_ptr; void Referee_Set_Robot_State(void) { - Referee_Robot_State.Game_Type = Referee_System.Game_Status.Type; - Referee_Robot_State.ID = Referee_System.Robot_State.ID; - Referee_Robot_State.Level = Referee_System.Robot_State.Level; + if (Referee_System.Online_Flag) + { + Referee_Robot_State.Game_Type = Referee_System.Game_Status.Type; + Referee_Robot_State.ID = Referee_System.Robot_State.ID; + Referee_Robot_State.Level = Referee_System.Robot_State.Level; + + Referee_Robot_State.Cooling_Rate = Referee_System.Robot_State.Shooter_Cooling_Value; + Referee_Robot_State.Heat_Max = Referee_System.Robot_State.Shooter_Heat_Max; + Referee_Robot_State.Launch_Speed_Max = DEFAULT_STANDARD_LAUNCH_SPEED_MAX; + Referee_Robot_State.Chassis_Power_Max = Referee_System.Robot_State.Chassis_Power_Max; - Referee_Robot_State.Cooling_Rate = Referee_System.Robot_State.Shooter_Cooling_Value; - Referee_Robot_State.Heat_Max = Referee_System.Robot_State.Shooter_Heat_Max; - Referee_Robot_State.Launch_Speed_Max = V3_STANDARD_LAUNCH_SPEED_MAX; - Referee_Robot_State.Chassis_Power_Max = Referee_System.Robot_State.Chassis_Power_Max; + Referee_Robot_State.Chassis_Power = Referee_System.Power_Heat.Chassis_Power; + Referee_Robot_State.Power_Buffer = Referee_System.Power_Heat.Chassis_Power_Buffer; + Referee_Robot_State.Shooter_Heat_1 = Referee_System.Power_Heat.Shooter_1_17mm_Heat; + Referee_Robot_State.Shooter_Heat_2 = Referee_System.Power_Heat.Shooter_2_17mm_Heat; + Referee_Robot_State.Shooting_Frequency = Referee_System.Shooter.Frequency; + Referee_Robot_State.Shooting_Speed = Referee_System.Shooter.Speed; + } + else + { + #ifdef ROBOT_TYPE_STANDARD + 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 - Referee_Robot_State.Chassis_Power = Referee_System.Power_Heat.Chassis_Power; - Referee_Robot_State.Power_Buffer = Referee_System.Power_Heat.Chassis_Power_Buffer; - Referee_Robot_State.Shooter_Heat_1 = Referee_System.Power_Heat.Shooter_1_17mm_Heat; - Referee_Robot_State.Shooter_Heat_2 = Referee_System.Power_Heat.Shooter_2_17mm_Heat; - Referee_Robot_State.Shooting_Frequency = Referee_System.Shooter.Frequency; - Referee_Robot_State.Shooting_Speed = Referee_System.Shooter.Speed; + #ifdef ROBOT_TYPE_HERO + Referee_Robot_State.Cooling_Rate = DEFAULT_HERO_COOLING_RATE+(Referee_Robot_State.Manual_Level-1)*8; + Referee_Robot_State.Heat_Max = DEFAULT_HERO_HEAT_MAX+(Referee_Robot_State.Manual_Level-1)*30; + Referee_Robot_State.Launch_Speed_Max = DEFAULT_HERO_LAUNCH_SPEED_MAX; + Referee_Robot_State.Chassis_Power_Max = DEFAULT_HERO_POWER_MAX+(Referee_Robot_State.Manual_Level-1)*5; + #endif + + #ifdef ROBOT_TYPE_SENTRY + Referee_Robot_State.Cooling_Rate = DEFAULT_SENTRY_COOLING_RATE; + Referee_Robot_State.Heat_Max = DEFAULT_SENTRY_HEAT_MAX; + Referee_Robot_State.Launch_Speed_Max = DEFAULT_SENTRY_LAUNCH_SPEED_MAX; + Referee_Robot_State.Chassis_Power_Max = DEFAULT_SENTRY_POWER_MAX; + #endif + } } void Referee_System_Timeout_Callback() { // Attemp to reinitialize UART service UART_Service_Init(g_referee_uart_instance_ptr); + Referee_System.Online_Flag = 0; } void Referee_System_Init(UART_HandleTypeDef *huart) { Referee_System.huart = huart; - HAL_UART_Receive_DMA(huart, Referee_System.Buffer, REFEREE_BUFFER_LEN); + Referee_Robot_State.Manual_Level = 1; g_referee_uart_instance_ptr = UART_Register(huart, Referee_System.Buffer, REFEREE_BUFFER_LEN, Referee_Get_Data); @@ -62,6 +90,11 @@ void Referee_Get_Data(UART_Instance_t *uart_instance) if (Referee_System.Buffer[n] == REFEREE_FRAME_HEADER_START) { 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/devices/src/supercap.c b/src/devices/src/supercap.c new file mode 100644 index 00000000..55978454 --- /dev/null +++ b/src/devices/src/supercap.c @@ -0,0 +1,27 @@ +#include "supercap.h" +Supercap_t g_supercap; +CAN_Instance_t *supercap_can_instance; + +void Supercap_Init(Supercap_t *g_supercap) +{ + // Initialize supercap + g_supercap->can_bus = 1; + g_supercap->tx_id = 0x166; + g_supercap->rx_id = 0x188; + supercap_can_instance = CAN_Device_Register(g_supercap->can_bus,g_supercap->tx_id,g_supercap->rx_id,Supercap_Decode); +} + +void Supercap_Decode(CAN_Instance_t *can_instance) +{ + // Send supercap data + uint8_t *data = can_instance->rx_buffer; + g_supercap.supercap_percent = data[0]; +} + +void Supercap_Send(void) +{ + // Send supercap data + uint8_t *data = supercap_can_instance->tx_buffer; + data[0] = 45;//Referee_Robot_State.Chassis_Power_Max; + CAN_Transmit(supercap_can_instance); +} \ No newline at end of file diff --git a/src/ui/inc/ui.h b/src/ui/inc/ui.h index b78f07cc..2de4431b 100644 --- a/src/ui/inc/ui.h +++ b/src/ui/inc/ui.h @@ -21,92 +21,101 @@ extern "C" { #include "ui_indicator_0_8.h" #include "ui_indicator_0_9.h" #include "ui_indicator_0_10.h" +#include "ui_indicator_0_11.h" -#define ui_init_indicator_0() \ -_ui_init_indicator_0_0(); \ -osDelay(100); \ -_ui_init_indicator_0_1(); \ -osDelay(100); \ -_ui_init_indicator_0_2(); \ -osDelay(100); \ -_ui_init_indicator_0_3(); \ -osDelay(100); \ -_ui_init_indicator_0_4(); \ -osDelay(100); \ -_ui_init_indicator_0_5(); \ -osDelay(100); \ -_ui_init_indicator_0_6(); \ -osDelay(100); \ -_ui_init_indicator_0_7(); \ -osDelay(100); \ -_ui_init_indicator_0_8(); \ -osDelay(100); \ -_ui_init_indicator_0_9(); \ -osDelay(100); \ -_ui_init_indicator_0_10();\ -osDelay(100) +#define SEND_INTERVAL_MS (40) + +#define ui_init_indicator_0() \ +_ui_init_indicator_0_0(); \ +osDelay(SEND_INTERVAL_MS); \ +_ui_init_indicator_0_1(); \ +osDelay(SEND_INTERVAL_MS); \ +_ui_init_indicator_0_2(); \ +osDelay(SEND_INTERVAL_MS); \ +_ui_init_indicator_0_3(); \ +osDelay(SEND_INTERVAL_MS); \ +_ui_init_indicator_0_4(); \ +osDelay(SEND_INTERVAL_MS); \ +_ui_init_indicator_0_5(); \ +osDelay(SEND_INTERVAL_MS); \ +_ui_init_indicator_0_6(); \ +osDelay(SEND_INTERVAL_MS); \ +_ui_init_indicator_0_7(); \ +osDelay(SEND_INTERVAL_MS); \ +_ui_init_indicator_0_8(); \ +osDelay(SEND_INTERVAL_MS); \ +_ui_init_indicator_0_9(); \ +osDelay(SEND_INTERVAL_MS); \ +_ui_init_indicator_0_10(); \ +osDelay(SEND_INTERVAL_MS); \ +_ui_init_indicator_0_11(); \ +osDelay(SEND_INTERVAL_MS) #define ui_update_indicator_0() \ -_ui_update_indicator_0_0(); \ -osDelay(100); \ -_ui_update_indicator_0_1(); \ -osDelay(100); \ -_ui_update_indicator_0_2(); \ -osDelay(100); \ -_ui_update_indicator_0_3(); \ -osDelay(100); \ -_ui_update_indicator_0_4(); \ -osDelay(100); \ -_ui_update_indicator_0_5(); \ -osDelay(100); \ -_ui_update_indicator_0_6(); \ -osDelay(100); \ -_ui_update_indicator_0_7(); \ -osDelay(100); \ -_ui_update_indicator_0_8(); \ -osDelay(100); \ -_ui_update_indicator_0_9(); \ -osDelay(100); \ -_ui_update_indicator_0_10();\ -osDelay(100) +_ui_update_indicator_0_0(); \ +osDelay(SEND_INTERVAL_MS); \ +_ui_update_indicator_0_1(); \ +osDelay(SEND_INTERVAL_MS); \ +_ui_update_indicator_0_2(); \ +osDelay(SEND_INTERVAL_MS); \ +_ui_update_indicator_0_3(); \ +osDelay(SEND_INTERVAL_MS); \ +_ui_update_indicator_0_4(); \ +osDelay(SEND_INTERVAL_MS); \ +_ui_update_indicator_0_5(); \ +osDelay(SEND_INTERVAL_MS); \ +_ui_update_indicator_0_6(); \ +osDelay(SEND_INTERVAL_MS); \ +_ui_update_indicator_0_7(); \ +osDelay(SEND_INTERVAL_MS); \ +_ui_update_indicator_0_8(); \ +osDelay(SEND_INTERVAL_MS); \ +_ui_update_indicator_0_9(); \ +osDelay(SEND_INTERVAL_MS); \ +_ui_update_indicator_0_10(); \ +osDelay(SEND_INTERVAL_MS); \ +_ui_update_indicator_0_11(); \ +osDelay(SEND_INTERVAL_MS) #define ui_remove_indicator_0() \ -_ui_remove_indicator_0_0(); \ -osDelay(100); \ -_ui_remove_indicator_0_1(); \ -osDelay(100); \ -_ui_remove_indicator_0_2(); \ -osDelay(100); \ -_ui_remove_indicator_0_3(); \ -osDelay(100); \ -_ui_remove_indicator_0_4(); \ -osDelay(100); \ -_ui_remove_indicator_0_5(); \ -osDelay(100); \ -_ui_remove_indicator_0_6(); \ -osDelay(100); \ -_ui_remove_indicator_0_7(); \ -osDelay(100); \ -_ui_remove_indicator_0_8(); \ -osDelay(100); \ -_ui_remove_indicator_0_9(); \ -osDelay(100); \ -_ui_remove_indicator_0_10();\ -osDelay(100) +_ui_remove_indicator_0_0(); \ +osDelay(SEND_INTERVAL_MS); \ +_ui_remove_indicator_0_1(); \ +osDelay(SEND_INTERVAL_MS); \ +_ui_remove_indicator_0_2(); \ +osDelay(SEND_INTERVAL_MS); \ +_ui_remove_indicator_0_3(); \ +osDelay(SEND_INTERVAL_MS); \ +_ui_remove_indicator_0_4(); \ +osDelay(SEND_INTERVAL_MS); \ +_ui_remove_indicator_0_5(); \ +osDelay(SEND_INTERVAL_MS); \ +_ui_remove_indicator_0_6(); \ +osDelay(SEND_INTERVAL_MS); \ +_ui_remove_indicator_0_7(); \ +osDelay(SEND_INTERVAL_MS); \ +_ui_remove_indicator_0_8(); \ +osDelay(SEND_INTERVAL_MS); \ +_ui_remove_indicator_0_9(); \ +osDelay(SEND_INTERVAL_MS); \ +_ui_remove_indicator_0_10(); \ +osDelay(SEND_INTERVAL_MS); \ +_ui_remove_indicator_0_11(); \ +osDelay(SEND_INTERVAL_MS) #include "ui_indicator_1_0.h" -#define ui_init_indicator_1() \ -_ui_init_indicator_1_0(); \ -osDelay(100) +#define ui_init_indicator_1() \ +_ui_init_indicator_1_0(); \ +osDelay(SEND_INTERVAL_MS) #define ui_update_indicator_1() \ _ui_update_indicator_1_0(); \ -osDelay(100) +osDelay(SEND_INTERVAL_MS) #define ui_remove_indicator_1() \ _ui_remove_indicator_1_0(); \ -osDelay(100) +osDelay(SEND_INTERVAL_MS) #ifdef __cplusplus } diff --git a/src/ui/inc/ui_indicator_0_11.h b/src/ui/inc/ui_indicator_0_11.h new file mode 100644 index 00000000..884e754b --- /dev/null +++ b/src/ui/inc/ui_indicator_0_11.h @@ -0,0 +1,16 @@ +// +// Created by RM UI Designer +// + +#ifndef UI_indicator_0_11_H +#define UI_indicator_0_11_H + +#include "ui_interface.h" + +extern ui_interface_string_t *ui_indicator_0_Level_Text; + +void _ui_init_indicator_0_11(); +void _ui_update_indicator_0_11(); +void _ui_remove_indicator_0_11(); + +#endif //UI_indicator_0_11_H diff --git a/src/ui/inc/ui_indicator_1_0.h b/src/ui/inc/ui_indicator_1_0.h index 170bb47f..d540338c 100644 --- a/src/ui/inc/ui_indicator_1_0.h +++ b/src/ui/inc/ui_indicator_1_0.h @@ -13,6 +13,7 @@ extern ui_interface_rect_t *ui_indicator_1_Autoaim_Select; extern ui_interface_number_t *ui_indicator_1_Supercap; extern ui_interface_line_t *ui_indicator_1_Aim_V_Line; extern ui_interface_line_t *ui_indicator_1_Aim_H_Line; +extern ui_interface_number_t *ui_indicator_1_Level_Indicator; void _ui_init_indicator_1_0(); void _ui_update_indicator_1_0(); diff --git a/src/ui/inc/ui_interface.h b/src/ui/inc/ui_interface.h index 5ac3127a..20854d79 100644 --- a/src/ui/inc/ui_interface.h +++ b/src/ui/inc/ui_interface.h @@ -10,12 +10,14 @@ #include "bsp_crc.h" #include "bsp_uart.h" #include "bsp_serial.h" +#include "referee_system.h" extern int ui_self_id; +extern UART_Instance_t *g_referee_uart_instance_ptr; void print_message(const uint8_t* message, int length); -#define SEND_MESSAGE(message, length) HAL_UART_Transmit_DMA(&huart1,message,length) //print_message(message, length) +#define SEND_MESSAGE(message, length) UART_Transmit(g_referee_uart_instance_ptr,message,length,UART_DMA) //print_message(message, length) void ui_proc_1_frame(ui_1_frame_t *msg); void ui_proc_2_frame(ui_2_frame_t *msg); diff --git a/src/ui/src/ui_indicator_0_11.c b/src/ui/src/ui_indicator_0_11.c new file mode 100644 index 00000000..a65c2a47 --- /dev/null +++ b/src/ui/src/ui_indicator_0_11.c @@ -0,0 +1,47 @@ +// +// Created by RM UI Designer +// + +#include "ui_indicator_0_11.h" +#include "string.h" + +#define FRAME_ID 1 +#define GROUP_ID 0 +#define START_ID 12 + +ui_string_frame_t ui_indicator_0_11; + +ui_interface_string_t* ui_indicator_0_Level_Text = &ui_indicator_0_11.option; + +void _ui_init_indicator_0_11() { + ui_indicator_0_11.option.figure_name[0] = FRAME_ID; + ui_indicator_0_11.option.figure_name[1] = GROUP_ID; + ui_indicator_0_11.option.figure_name[2] = START_ID; + ui_indicator_0_11.option.operate_tpyel = 1; + ui_indicator_0_11.option.figure_tpye = 7; + ui_indicator_0_11.option.layer = 0; + ui_indicator_0_11.option.font_size = 20; + ui_indicator_0_11.option.start_x = 900; + ui_indicator_0_11.option.start_y = 100; + ui_indicator_0_11.option.color = 1; + ui_indicator_0_11.option.str_length = 6; + ui_indicator_0_11.option.width = 2; + strcpy(ui_indicator_0_Level_Text->string, "LEVEL:"); + + ui_proc_string_frame(&ui_indicator_0_11); + SEND_MESSAGE((uint8_t *) &ui_indicator_0_11, sizeof(ui_indicator_0_11)); +} + +void _ui_update_indicator_0_11() { + ui_indicator_0_11.option.operate_tpyel = 2; + + ui_proc_string_frame(&ui_indicator_0_11); + SEND_MESSAGE((uint8_t *) &ui_indicator_0_11, sizeof(ui_indicator_0_11)); +} + +void _ui_remove_indicator_0_11() { + ui_indicator_0_11.option.operate_tpyel = 3; + + ui_proc_string_frame(&ui_indicator_0_11); + SEND_MESSAGE((uint8_t *) &ui_indicator_0_11, sizeof(ui_indicator_0_11)); +} \ No newline at end of file diff --git a/src/ui/src/ui_indicator_1_0.c b/src/ui/src/ui_indicator_1_0.c index c14f06c9..bc000e7c 100644 --- a/src/ui/src/ui_indicator_1_0.c +++ b/src/ui/src/ui_indicator_1_0.c @@ -7,7 +7,7 @@ #define FRAME_ID 1 #define GROUP_ID 1 #define START_ID 0 -#define OBJ_NUM 6 +#define OBJ_NUM 7 #define FRAME_OBJ_NUM 7 CAT(ui_, CAT(FRAME_OBJ_NUM, _frame_t)) ui_indicator_1_0; @@ -17,6 +17,7 @@ ui_interface_rect_t *ui_indicator_1_Autoaim_Select = (ui_interface_rect_t *)&(ui ui_interface_number_t *ui_indicator_1_Supercap = (ui_interface_number_t *)&(ui_indicator_1_0.data[3]); ui_interface_line_t *ui_indicator_1_Aim_V_Line = (ui_interface_line_t *)&(ui_indicator_1_0.data[4]); ui_interface_line_t *ui_indicator_1_Aim_H_Line = (ui_interface_line_t *)&(ui_indicator_1_0.data[5]); +ui_interface_number_t *ui_indicator_1_Level_Indicator = (ui_interface_number_t *)&(ui_indicator_1_0.data[6]); void _ui_init_indicator_1_0() { for (int i = 0; i < OBJ_NUM; i++) { @@ -82,6 +83,15 @@ void _ui_init_indicator_1_0() { ui_indicator_1_Aim_H_Line->end_y = 470; ui_indicator_1_Aim_H_Line->color = 3; ui_indicator_1_Aim_H_Line->width = 2; + + ui_indicator_1_Level_Indicator->figure_tpye = 6; + ui_indicator_1_Level_Indicator->layer = 1; + ui_indicator_1_Level_Indicator->font_size = 20; + ui_indicator_1_Level_Indicator->start_x = 1020; + ui_indicator_1_Level_Indicator->start_y = 100; + ui_indicator_1_Level_Indicator->color = 1; + ui_indicator_1_Level_Indicator->number = 1; + ui_indicator_1_Level_Indicator->width = 2; CAT(ui_proc_, CAT(FRAME_OBJ_NUM, _frame))(&ui_indicator_1_0); SEND_MESSAGE((uint8_t *) &ui_indicator_1_0, sizeof(ui_indicator_1_0));