diff --git a/Makefile b/Makefile index 436d8c9..23a4894 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 \ diff --git a/src/algo/src/Swerve_Locomotion.c b/src/algo/src/Swerve_Locomotion.c index d44c6b9..5c2b62a 100644 --- a/src/algo/src/Swerve_Locomotion.c +++ b/src/algo/src/Swerve_Locomotion.c @@ -244,7 +244,7 @@ void Swerve_Drive(float x, float y, float omega) { __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)) + 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; diff --git a/src/app/inc/launch_task.h b/src/app/inc/launch_task.h index c054aab..84c5db8 100644 --- a/src/app/inc/launch_task.h +++ b/src/app/inc/launch_task.h @@ -4,18 +4,18 @@ #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 (15) +#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 +//#define HEAT_CONTROL typedef struct { diff --git a/src/app/inc/robot_tasks.h b/src/app/inc/robot_tasks.h index c778ea5..8494245 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/launch_task.c b/src/app/src/launch_task.c index a496312..a85430e 100644 --- a/src/app/src/launch_task.c +++ b/src/app/src/launch_task.c @@ -93,14 +93,10 @@ void Feed_Angle_Calc() #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.heat_count*2 % 100 == 0) { - g_launch_target.calculated_heat = Referee_Robot_State.Shooter_Heat_1; + 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) { @@ -111,31 +107,30 @@ void Feed_Angle_Calc() { 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); + } } + 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_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); - } + #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 760cf12..58119ed 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 6235932..54dc263 100644 --- a/src/app/src/robot.c +++ b/src/app/src/robot.c @@ -15,6 +15,7 @@ #include "ui.h" #include "user_math.h" #include "Swerve_Locomotion.h" +#include "supercap.h" extern DJI_Motor_Handle_t *g_yaw; @@ -26,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; @@ -49,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(); } @@ -137,7 +140,7 @@ void Robot_Cmd_Loop() /* Launch control starts here */ g_launch_target.heat_count++; - if (Referee_Robot_State.Shooter_Heat_1 < Referee_Robot_State.Heat_Max) + 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 diff --git a/src/devices/inc/motor.h b/src/devices/inc/motor.h index 64597ed..b13369f 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/supercap.h b/src/devices/inc/supercap.h new file mode 100644 index 0000000..7db43b2 --- /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/supercap.c b/src/devices/src/supercap.c new file mode 100644 index 0000000..5597845 --- /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 cf798ee..2de4431 100644 --- a/src/ui/inc/ui.h +++ b/src/ui/inc/ui.h @@ -23,97 +23,99 @@ extern "C" { #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); \ -_ui_init_indicator_0_11();\ -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_11();\ -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_11();\ -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_interface.h b/src/ui/inc/ui_interface.h index 5ac3127..20854d7 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);