diff --git a/src/algo/inc/user_math.h b/src/algo/inc/user_math.h index 6ffd307c..4795568f 100644 --- a/src/algo/inc/user_math.h +++ b/src/algo/inc/user_math.h @@ -39,7 +39,7 @@ } \ } while (0); -#define BUFFER_SIZE (1000) +#define BUFFER_SIZE (500) #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 5f69f517..d180cace 100644 --- a/src/algo/src/Swerve_Locomotion.c +++ b/src/algo/src/Swerve_Locomotion.c @@ -53,13 +53,23 @@ void Swerve_Init() // init common PID configuration for azimuth motors Motor_Config_t azimuth_motor_config = { - .control_mode = POSITION_CONTROL, - .angle_pid = { - .kp = 15000.0f, - .kd = 8000.0f, - .kf = 5000.0f, - .output_limit = GM6020_MAX_CURRENT, - }}; + .control_mode = POSITION_VELOCITY_SERIES, + .angle_pid = + { + .kp = 200.0f, + .kd = 50.0f, + .output_limit = 100.0f, + }, + .velocity_pid = + { + .kp = 200.0f, + .ki = 0.0f, + .kf = 0.0f, + .feedforward_limit = 5000.0f, + .integral_limit = 5000.0f, + .output_limit = GM6020_MAX_CURRENT, + }, + }; // init common PID configuration for drive motors Motor_Config_t drive_motor_config = { @@ -240,31 +250,32 @@ void Swerve_Drive(float x, float y, float omega) #ifdef POWER_REGULATION if(Referee_System.Online_Flag) { - 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 - { - 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); + // 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))) + // g_robot_state.power_increment_ratio += 0.001f; + // else + // g_robot_state.power_increment_ratio -= 0.001f; + // } + // 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; + // } + // __MAX_LIMIT(g_robot_state.power_increment_ratio, 0.6f, 3.0f); + g_robot_state.power_increment_ratio = 1 + Referee_Robot_State.Level*0.08f + g_supercap.supercap_enabled_flag*0.7f; 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 { - g_robot_state.power_increment_ratio = 1 + Referee_Robot_State.Manual_Level*0.1f; + g_robot_state.power_increment_ratio = 1 + Referee_Robot_State.Manual_Level*0.08f + g_supercap.supercap_enabled_flag*0.7f; 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 diff --git a/src/app/inc/launch_task.h b/src/app/inc/launch_task.h index cd3167fa..fe9fe4a9 100644 --- a/src/app/inc/launch_task.h +++ b/src/app/inc/launch_task.h @@ -25,11 +25,15 @@ typedef struct uint8_t single_launch_flag; uint8_t single_launch_finished_flag; uint8_t burst_launch_flag; + uint8_t prev_burst_launch_flag; uint8_t flywheel_enabled; int16_t calculated_heat; uint16_t heat_count; uint16_t launch_freq_count; + + uint8_t reverse_flag; + uint8_t prev_reverse_flag; } 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 409cd254..3c93fd2f 100644 --- a/src/app/src/chassis_task.c +++ b/src/app/src/chassis_task.c @@ -6,7 +6,8 @@ #include "imu_task.h" #include "Swerve_Locomotion.h" -#define SPINTOP_RAMP_COEF (0.03f) +#define SPINTOP_RAMP_COEF (0.01f) +#define TRANSLATION_RAMP_COEF (0.000001f) #define SPIN_TOP_OMEGA (1.0f) extern Robot_State_t g_robot_state; @@ -33,7 +34,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*0.2f + chassis_rad * SPIN_TOP_OMEGA); + float spin_coeff = chassis_rad * SPIN_TOP_OMEGA / (translation_speed*0.5f + chassis_rad * SPIN_TOP_OMEGA); // ramp up to target omega float target_omega = SPIN_TOP_OMEGA * spin_coeff; @@ -44,6 +45,26 @@ void Chassis_Ctrl_Loop() g_robot_state.chassis_omega *= (1 - SPINTOP_RAMP_COEF); } + float chassis_x_speed = g_robot_state.chassis_x_speed; + if(chassis_x_speed > 0.01f) + { + g_robot_state.chassis_x_speed += TRANSLATION_RAMP_COEF * (chassis_x_speed - g_robot_state.chassis_x_speed); + } + else + { + g_robot_state.chassis_x_speed *= (1 - TRANSLATION_RAMP_COEF); + } + + float chassis_y_speed = g_robot_state.chassis_y_speed; + if(chassis_y_speed > 0.01f) + { + g_robot_state.chassis_y_speed += TRANSLATION_RAMP_COEF * (chassis_y_speed - g_robot_state.chassis_y_speed); + } + else + { + g_robot_state.chassis_y_speed *= (1 - TRANSLATION_RAMP_COEF); + } + Swerve_Drive(g_robot_state.chassis_x_speed, g_robot_state.chassis_y_speed, g_robot_state.chassis_omega); } else diff --git a/src/app/src/debug_task.c b/src/app/src/debug_task.c index f39ee068..6e95a921 100644 --- a/src/app/src/debug_task.c +++ b/src/app/src/debug_task.c @@ -30,12 +30,12 @@ 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; + //static uint32_t counter = 0; #ifdef PRINT_RUNTIME_STATS if (counter % 100 == 0) // Print every 100 cycles { @@ -49,12 +49,12 @@ void Debug_Task_Loop(void) // 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(&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); + DEBUG_PRINTF(&huart6, ">target_angle:%f\n>actual_angle:%f\n", + g_swerve_fr.azimuth_motor->angle_pid->ref,g_swerve_fr.azimuth_motor->stats->absolute_angle_rad); #endif } \ No newline at end of file diff --git a/src/app/src/launch_task.c b/src/app/src/launch_task.c index b9add44a..4f041d5f 100644 --- a/src/app/src/launch_task.c +++ b/src/app/src/launch_task.c @@ -56,7 +56,7 @@ void Launch_Task_Init() { }, .angle_pid = { - .kp = 400000.0f, + .kp = 450000.0f, .kd = 15000000.0f, .ki = 0.1f, .output_limit = M2006_MAX_CURRENT, @@ -102,7 +102,7 @@ void Feed_Angle_Calc() 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.burst_launch_flag && !g_launch_target.reverse_flag) { if (g_launch_target.launch_freq_count*2 > LAUNCH_PERIOD) { @@ -114,8 +114,15 @@ void Feed_Angle_Calc() } } 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_Angle(g_motor_feed,g_launch_target.feed_angle); + } + if(g_launch_target.reverse_flag && !g_launch_target.prev_reverse_flag) + { + 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) { diff --git a/src/app/src/robot.c b/src/app/src/robot.c index 74111bfd..ed05d07d 100644 --- a/src/app/src/robot.c +++ b/src/app/src/robot.c @@ -51,7 +51,7 @@ 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(); @@ -140,7 +140,8 @@ 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-20)) + g_launch_target.prev_burst_launch_flag = g_launch_target.burst_launch_flag; + if (Referee_Robot_State.Shooter_Heat_1 < (Referee_Robot_State.Heat_Max-10)) { if (g_remote.controller.wheel < -50.0f) { // dial wheel forward single fire @@ -188,7 +189,7 @@ void Robot_Cmd_Loop() Referee_Robot_State.Manual_Level++; __MAX_LIMIT(Referee_Robot_State.Manual_Level,1,10); } - if (g_remote.keyboard.Shift == 1) + if (g_remote.keyboard.Shift) { g_supercap.supercap_enabled_flag = 1; } @@ -196,6 +197,14 @@ void Robot_Cmd_Loop() { g_supercap.supercap_enabled_flag = 0; } + if(g_remote.controller.wheel > 50.0f && !g_launch_target.flywheel_enabled) + { + g_supercap.supercap_enabled_flag = 1; + } + else + { + g_supercap.supercap_enabled_flag = 0; + } 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; @@ -204,13 +213,16 @@ void Robot_Cmd_Loop() /* Keyboard Toggles Start Here */ /* AutoAiming Flag, not used only for debug */ - if ((g_remote.mouse.right == 1) || (g_remote.controller.right_switch == UP)) + g_launch_target.prev_reverse_flag = g_launch_target.reverse_flag; + if (g_remote.mouse.right == 1 || (g_remote.controller.wheel < -50.0f && g_launch_target.flywheel_enabled) + || (!g_launch_target.burst_launch_flag && g_launch_target.prev_burst_launch_flag)) { - g_robot_state.autoaiming_enabled = 1; + g_launch_target.reverse_flag = 1; + g_launch_target.calculated_heat = Referee_Robot_State.Shooter_Heat_1; } else { - g_robot_state.autoaiming_enabled = 0; + g_launch_target.reverse_flag = 0; } if(Referee_System.Robot_State.Chassis_Power_Output == 0) diff --git a/src/devices/inc/supercap.h b/src/devices/inc/supercap.h index ebb40dfd..cf315c5d 100644 --- a/src/devices/inc/supercap.h +++ b/src/devices/inc/supercap.h @@ -6,6 +6,8 @@ #include "referee_system.h" #include "jetson_orin.h" +#define SUPERCAP_POWER (20) + typedef struct { uint8_t can_bus; diff --git a/src/devices/src/jetson_orin.c b/src/devices/src/jetson_orin.c index 7c6f4b59..249ac799 100644 --- a/src/devices/src/jetson_orin.c +++ b/src/devices/src/jetson_orin.c @@ -104,5 +104,5 @@ void Jetson_Orin_Send_Data(void) g_orin_data.tx_buffer[1] = g_orin_data.sending.enemy_color_flag << 1 | g_orin_data.sending.game_start_flag; memcpy(&g_orin_data.tx_buffer[2],&g_orin_data.sending.float_byte.data_bytes[0], 32*sizeof(uint8_t)); - UART_Transmit(g_orin_uart_instance_ptr, g_orin_data.tx_buffer, sizeof(g_orin_data.tx_buffer), UART_DMA); + //UART_Transmit(g_orin_uart_instance_ptr, g_orin_data.tx_buffer, sizeof(g_orin_data.tx_buffer), UART_DMA); } \ No newline at end of file diff --git a/src/devices/src/referee_system.c b/src/devices/src/referee_system.c index d141eee5..11ca9957 100644 --- a/src/devices/src/referee_system.c +++ b/src/devices/src/referee_system.c @@ -26,7 +26,7 @@ void Referee_Set_Robot_State(void) 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 + g_supercap.supercap_enabled_flag*40.0f;; + 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; diff --git a/src/devices/src/supercap.c b/src/devices/src/supercap.c index ad2eef5d..fabaf3d9 100644 --- a/src/devices/src/supercap.c +++ b/src/devices/src/supercap.c @@ -17,7 +17,7 @@ void Supercap_Decode(CAN_Instance_t *can_instance) { // Send supercap data uint8_t *data = can_instance->rx_buffer; - g_supercap.supercap_percent = (data[0]-20)*2; + g_supercap.supercap_percent = (data[0]-30)*3.0f; } void Supercap_Send(void)