diff --git a/src/algo/src/Swerve_Locomotion.c b/src/algo/src/Swerve_Locomotion.c index d180cace..4e7c90f8 100644 --- a/src/algo/src/Swerve_Locomotion.c +++ b/src/algo/src/Swerve_Locomotion.c @@ -268,14 +268,14 @@ void Swerve_Drive(float x, float y, float omega) // 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; + g_robot_state.power_increment_ratio = 0.9f + Referee_Robot_State.Level*0.11f + g_supercap.supercap_enabled_flag*0.6f; 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.08f + g_supercap.supercap_enabled_flag*0.7f; + g_robot_state.power_increment_ratio = 0.9f + Referee_Robot_State.Manual_Level*0.11f + g_supercap.supercap_enabled_flag*0.6f; 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/src/chassis_task.c b/src/app/src/chassis_task.c index 3c93fd2f..f5e9377f 100644 --- a/src/app/src/chassis_task.c +++ b/src/app/src/chassis_task.c @@ -6,8 +6,8 @@ #include "imu_task.h" #include "Swerve_Locomotion.h" -#define SPINTOP_RAMP_COEF (0.01f) -#define TRANSLATION_RAMP_COEF (0.000001f) +#define SPINTOP_RAMP_COEF (0.05f) +#define TRANSLATION_RAMP_COEF (0.05f) #define SPIN_TOP_OMEGA (1.0f) extern Robot_State_t g_robot_state; @@ -34,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.5f + chassis_rad * SPIN_TOP_OMEGA); + float spin_coeff = chassis_rad * SPIN_TOP_OMEGA / (translation_speed + chassis_rad * SPIN_TOP_OMEGA); // ramp up to target omega float target_omega = SPIN_TOP_OMEGA * spin_coeff; @@ -44,26 +44,9 @@ 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); - } + //g_robot_state.chassis_omega = (1-SPINTOP_RAMP_COEF)*g_robot_state.chassis_omega + SPINTOP_RAMP_COEF*g_robot_state.chassis_omega; + g_robot_state.chassis_x_speed = (1 - TRANSLATION_RAMP_COEF)*g_robot_state.chassis_x_speed + TRANSLATION_RAMP_COEF*g_robot_state.chassis_x_speed; + g_robot_state.chassis_y_speed = (1 - TRANSLATION_RAMP_COEF)*g_robot_state.chassis_y_speed + TRANSLATION_RAMP_COEF*g_robot_state.chassis_y_speed; Swerve_Drive(g_robot_state.chassis_x_speed, g_robot_state.chassis_y_speed, g_robot_state.chassis_omega); } diff --git a/src/app/src/robot.c b/src/app/src/robot.c index ed05d07d..9f04b770 100644 --- a/src/app/src/robot.c +++ b/src/app/src/robot.c @@ -189,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) + if (g_remote.keyboard.Shift == 1 || (g_remote.controller.wheel > 50.0f && !g_launch_target.flywheel_enabled)) { g_supercap.supercap_enabled_flag = 1; } @@ -197,14 +197,6 @@ 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; diff --git a/src/devices/src/supercap.c b/src/devices/src/supercap.c index fabaf3d9..b47a1b3d 100644 --- a/src/devices/src/supercap.c +++ b/src/devices/src/supercap.c @@ -24,14 +24,7 @@ void Supercap_Send(void) { // Send supercap data uint8_t *data = supercap_can_instance->tx_buffer; - if (g_orin_data.sending.game_start_flag) - { - data[0] = Referee_Robot_State.Chassis_Power_Max; - } - else - { - data[0] = 90; - } + data[0] = Referee_Robot_State.Chassis_Power_Max; CAN_Transmit(supercap_can_instance); } \ No newline at end of file