From 23bc853ba614a60d99c680e83abfa29141886bd3 Mon Sep 17 00:00:00 2001 From: nicolasshul <74377515+nicolasshul@users.noreply.github.com> Date: Tue, 17 Jun 2025 22:49:45 -0400 Subject: [PATCH 1/5] Initial commit for branch --- Swerve-Standard/src/chassis_task.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Swerve-Standard/src/chassis_task.c b/Swerve-Standard/src/chassis_task.c index 40bc404..fed589d 100644 --- a/Swerve-Standard/src/chassis_task.c +++ b/Swerve-Standard/src/chassis_task.c @@ -146,7 +146,7 @@ void Chassis_Ctrl_Loop() // g_chassis_state.v_x = g_chassis_state.v_x * cos(theta) - g_chassis_state.v_y * sin(theta); // g_chassis_state.v_y = g_chassis_state.v_x * sin(theta) + g_chassis_state.v_y * cos(theta); - // If spintop enabled, chassis omega set to spintop value + // If spintop enabled, chassis omega set to spintop value --> smthn here if (g_robot_state.chassis.IS_SPINTOP_ENABLED) { g_chassis_state.omega = rate_limiter_iterate(&chassis_omega_limiter, Rescale_Chassis_Velocity()); } else { @@ -232,4 +232,4 @@ float Rescale_Chassis_Velocity(void) { float spin_coeff = chassis_rad * g_spintop_omega / (translation_speed * 2.0f + chassis_rad * g_spintop_omega); float target_omega = g_spintop_omega * spin_coeff; return target_omega; -} \ No newline at end of file +} From 819fe23d907b3ffad9fd376a3b8fb692df7f4a03 Mon Sep 17 00:00:00 2001 From: nicolasshul Date: Wed, 18 Jun 2025 01:49:36 -0400 Subject: [PATCH 2/5] added functionality for different locked states when not in spintop --- Swerve-Standard/inc/robot.h | 8 ++++++ Swerve-Standard/src/chassis_task.c | 43 +++++++++++++++++++++++++++--- 2 files changed, 48 insertions(+), 3 deletions(-) diff --git a/Swerve-Standard/inc/robot.h b/Swerve-Standard/inc/robot.h index 4c6de26..91d1b56 100644 --- a/Swerve-Standard/inc/robot.h +++ b/Swerve-Standard/inc/robot.h @@ -11,12 +11,20 @@ typedef enum Robot_State_e { ENABLED } Robot_State_e; +typedef enum Locked_State_e { + RANDOM, + ANGLED, + STRAIGHT, + OFF +} Locked_State_e; + typedef struct { // chassis motion float x_speed; float y_speed; float omega; uint8_t IS_SPINTOP_ENABLED; + Locked_State_e locked_state; // power management uint16_t power_index; diff --git a/Swerve-Standard/src/chassis_task.c b/Swerve-Standard/src/chassis_task.c index fed589d..2c9be90 100644 --- a/Swerve-Standard/src/chassis_task.c +++ b/Swerve-Standard/src/chassis_task.c @@ -7,6 +7,7 @@ #include "referee_system.h" #include "swerve_locomotion.h" #include "rate_limiter.h" +#include "pid.h" extern Robot_State_t g_robot_state; extern Remote_t g_remote; @@ -18,6 +19,9 @@ swerve_constants_t g_swerve_constants; swerve_chassis_state_t g_chassis_state; float measured_angles[NUMBER_OF_MODULES]; +float gimbal_angle_difference; +PID_t angle_locking_pid; + rate_limiter_t chassis_vel_limiters[4]; rate_limiter_t chassis_omega_limiter; @@ -104,6 +108,9 @@ void Chassis_Task_Init() } #define SWERVE_MAX_OMEGA_ACCEL (5.0f) rate_limiter_init(&chassis_omega_limiter, SWERVE_MAX_OMEGA_ACCEL); + + // Initialize PID for locking + PID_Init(&angle_locking_pid, 20, 0, 2500, 2 * PI * 30, 0, 0); } void Chassis_Ctrl_Loop() @@ -146,11 +153,41 @@ void Chassis_Ctrl_Loop() // g_chassis_state.v_x = g_chassis_state.v_x * cos(theta) - g_chassis_state.v_y * sin(theta); // g_chassis_state.v_y = g_chassis_state.v_x * sin(theta) + g_chassis_state.v_y * cos(theta); - // If spintop enabled, chassis omega set to spintop value --> smthn here + // If spintop enabled, chassis omega set to spintop value + gimbal_angle_difference = g_robot_state.gimbal.yaw_angle if (g_robot_state.chassis.IS_SPINTOP_ENABLED) { g_chassis_state.omega = rate_limiter_iterate(&chassis_omega_limiter, Rescale_Chassis_Velocity()); - } else { - g_chassis_state.omega = rate_limiter_iterate(&chassis_omega_limiter, g_robot_state.chassis.omega * SWERVE_MAX_ANGLUAR_SPEED); + } + else if (g_robot_state.chassis.locked_state == STRAIGHT) { + __MAP_ANGLE_TO_UNIT_CIRCLE(gimbal_angle_difference); + + gimbal_angle_difference = gimbal_angle_difference % (PI / 2); + if (gimbal_angle_difference > PI / 4) { // might be easier to go by the way of spin top direction + gimbal_angle_difference = -1 * ((PI / 2) - gimbal_angle_difference); + } + chassis_omega_new_target = PID(&angle_locking_pid, gimbal_angle_difference); + __MAX_LIMIT(chassis_omega_new_target, -1 * g_spintop_omega, g_spintop_omega); // might not need this + g_chassis_state.omega = rate_limiter_iterate(&chassis_omega_limiter, chassis_omega_new_target); + } + else if (g_robot_state.chassis.locked_state == ANGLED) { + __MAP_ANGLE_TO_UNIT_CIRCLE(gimbal_angle_difference); + + gimbal_angle_difference += PI / 4; + gimbal_angle_difference = gimbal_angle_difference % (PI / 2); + if (gimbal_angle_difference > PI / 4) { + gimbal_angle_difference = -1 * ((PI / 2) - gimbal_angle_difference); + } + chassis_omega_new_target = PID(&angle_locking_pid, gimbal_angle_difference); + __MAX_LIMIT(chassis_omega_new_target, -6 * 2 * PI, 6 * 2 * PI); + g_chassis_state.omega = rate_limiter_iterate(&chassis_omega_limiter, chassis_omega_new_target); + } + else if (g_robot_state.chassis.locked_state == RANDOM) { + g_chassis_state.omega = rate_limiter_iterate(&chassis_omega_limiter, 0); + } + else { + // currently there is no way to change the ang. velo of the chassis through controller/keyboard, so g_robot_state.chassis.omega is always 0. + // switch locked_state to off when input is detected + g_chassis_state.omega = rate_limiter_iterate(&chassis_omega_limiter, g_robot_state.chassis.omega * SWERVE_MAX_ANGLUAR_SPEED); } // Calculate the kinematics of the chassis From dd60a03489198ac60c4dc390f8ee9404b13561bb Mon Sep 17 00:00:00 2001 From: nicolasshul <74377515+nicolasshul@users.noreply.github.com> Date: Wed, 18 Jun 2025 03:43:49 -0400 Subject: [PATCH 3/5] minor bug fixes --- Swerve-Standard/src/chassis_task.c | 10 ++++++---- control-base/algo/inc/user_math.h | 10 ++++++++++ 2 files changed, 16 insertions(+), 4 deletions(-) diff --git a/Swerve-Standard/src/chassis_task.c b/Swerve-Standard/src/chassis_task.c index 2c9be90..895472c 100644 --- a/Swerve-Standard/src/chassis_task.c +++ b/Swerve-Standard/src/chassis_task.c @@ -154,14 +154,16 @@ void Chassis_Ctrl_Loop() // g_chassis_state.v_y = g_chassis_state.v_x * sin(theta) + g_chassis_state.v_y * cos(theta); // If spintop enabled, chassis omega set to spintop value - gimbal_angle_difference = g_robot_state.gimbal.yaw_angle + gimbal_angle_difference = g_robot_state.gimbal.yaw_angle; + float chassis_omega_new_target; if (g_robot_state.chassis.IS_SPINTOP_ENABLED) { g_chassis_state.omega = rate_limiter_iterate(&chassis_omega_limiter, Rescale_Chassis_Velocity()); } else if (g_robot_state.chassis.locked_state == STRAIGHT) { __MAP_ANGLE_TO_UNIT_CIRCLE(gimbal_angle_difference); - gimbal_angle_difference = gimbal_angle_difference % (PI / 2); + // Compute mod + gimbal_angle_difference = __MOD_F(gimbal_angle_difference, PI / 2); if (gimbal_angle_difference > PI / 4) { // might be easier to go by the way of spin top direction gimbal_angle_difference = -1 * ((PI / 2) - gimbal_angle_difference); } @@ -173,12 +175,12 @@ void Chassis_Ctrl_Loop() __MAP_ANGLE_TO_UNIT_CIRCLE(gimbal_angle_difference); gimbal_angle_difference += PI / 4; - gimbal_angle_difference = gimbal_angle_difference % (PI / 2); + gimbal_angle_difference = __MOD_F(gimbal_angle_difference, PI / 2); if (gimbal_angle_difference > PI / 4) { gimbal_angle_difference = -1 * ((PI / 2) - gimbal_angle_difference); } chassis_omega_new_target = PID(&angle_locking_pid, gimbal_angle_difference); - __MAX_LIMIT(chassis_omega_new_target, -6 * 2 * PI, 6 * 2 * PI); + __MAX_LIMIT(chassis_omega_new_target, -1 * g_spintop_omega, g_spintop_omega); g_chassis_state.omega = rate_limiter_iterate(&chassis_omega_limiter, chassis_omega_new_target); } else if (g_robot_state.chassis.locked_state == RANDOM) { diff --git a/control-base/algo/inc/user_math.h b/control-base/algo/inc/user_math.h index 727d53d..e6ef97d 100644 --- a/control-base/algo/inc/user_math.h +++ b/control-base/algo/inc/user_math.h @@ -70,4 +70,14 @@ #define __SLEW_RATE_LIMIT(curr, delta, ramp) \ curr = (1 - (ramp)) * (curr) + (ramp) * (delta) +#define __ABS(x) \ + (x < 0 ? -1 * x : x) \ + +// floor for floats +#define __FLOOR_F(x) \ + (x < 0 && __ABS(x - ((int) x)) > 0.0001 ? (float)((int) x) - 1.0 : (float)((int) x)) \ + +#define __MOD_F(a, b) \ + a - b * __FLOOR_F((a / b)) \ + #endif // USER_MATH_H \ No newline at end of file From 36b029bb2b16ba5ddd3f127b75d59c447b198e61 Mon Sep 17 00:00:00 2001 From: nicolasshul <74377515+nicolasshul@users.noreply.github.com> Date: Wed, 18 Jun 2025 19:13:36 -0400 Subject: [PATCH 4/5] added keyboard inputs for different locked states --- Swerve-Standard/inc/robot.h | 3 +-- Swerve-Standard/src/chassis_task.c | 10 ++-------- Swerve-Standard/src/robot.c | 12 ++++++++++++ 3 files changed, 15 insertions(+), 10 deletions(-) diff --git a/Swerve-Standard/inc/robot.h b/Swerve-Standard/inc/robot.h index 91d1b56..ec2d643 100644 --- a/Swerve-Standard/inc/robot.h +++ b/Swerve-Standard/inc/robot.h @@ -12,10 +12,9 @@ typedef enum Robot_State_e { } Robot_State_e; typedef enum Locked_State_e { - RANDOM, ANGLED, STRAIGHT, - OFF + RANDOM } Locked_State_e; typedef struct { diff --git a/Swerve-Standard/src/chassis_task.c b/Swerve-Standard/src/chassis_task.c index 895472c..6314718 100644 --- a/Swerve-Standard/src/chassis_task.c +++ b/Swerve-Standard/src/chassis_task.c @@ -162,7 +162,6 @@ void Chassis_Ctrl_Loop() else if (g_robot_state.chassis.locked_state == STRAIGHT) { __MAP_ANGLE_TO_UNIT_CIRCLE(gimbal_angle_difference); - // Compute mod gimbal_angle_difference = __MOD_F(gimbal_angle_difference, PI / 2); if (gimbal_angle_difference > PI / 4) { // might be easier to go by the way of spin top direction gimbal_angle_difference = -1 * ((PI / 2) - gimbal_angle_difference); @@ -183,15 +182,10 @@ void Chassis_Ctrl_Loop() __MAX_LIMIT(chassis_omega_new_target, -1 * g_spintop_omega, g_spintop_omega); g_chassis_state.omega = rate_limiter_iterate(&chassis_omega_limiter, chassis_omega_new_target); } - else if (g_robot_state.chassis.locked_state == RANDOM) { + else { // random locking g_chassis_state.omega = rate_limiter_iterate(&chassis_omega_limiter, 0); } - else { - // currently there is no way to change the ang. velo of the chassis through controller/keyboard, so g_robot_state.chassis.omega is always 0. - // switch locked_state to off when input is detected - g_chassis_state.omega = rate_limiter_iterate(&chassis_omega_limiter, g_robot_state.chassis.omega * SWERVE_MAX_ANGLUAR_SPEED); - } - + // Calculate the kinematics of the chassis swerve_calculate_kinematics(&g_chassis_state, &g_swerve_constants); swerve_optimize_module_angles(&g_chassis_state, measured_angles); diff --git a/Swerve-Standard/src/robot.c b/Swerve-Standard/src/robot.c index 5231d74..58c1eb7 100644 --- a/Swerve-Standard/src/robot.c +++ b/Swerve-Standard/src/robot.c @@ -156,6 +156,18 @@ void Process_Remote_Input() g_robot_state.chassis.IS_SPINTOP_ENABLED ^= 0x01; } + if (g_remote.keyboard.R) { + g_robot_state.chassis.locked_state = RANDOM; + } + + if (g_remote.keyboard.E) { + g_robot_state.chassis.locked_state = STRAIGHT; + } + + if (g_remote.keyboard.Q) { + g_robot_state.chassis.locked_state = ANGLED; + } + if (g_remote.controller.left_switch == UP) { // Left switch high to enable spintop //g_robot_state.chassis.IS_SPINTOP_ENABLED = 1; //g_robtot_state.launch.IS_FIRING_ENABLED = 1; From 50f7502274a37a1ac112a0e14e7a64d2884f31a4 Mon Sep 17 00:00:00 2001 From: andyNiu Date: Wed, 18 Jun 2025 23:39:18 -0400 Subject: [PATCH 5/5] Currently Debugging the code and tuning PID --- Swerve-Standard/src/chassis_task.c | 77 +++++++++++++++++------------- Swerve-Standard/src/robot.c | 12 ++++- 2 files changed, 56 insertions(+), 33 deletions(-) diff --git a/Swerve-Standard/src/chassis_task.c b/Swerve-Standard/src/chassis_task.c index 6314718..ea6c1e4 100644 --- a/Swerve-Standard/src/chassis_task.c +++ b/Swerve-Standard/src/chassis_task.c @@ -12,6 +12,7 @@ extern Robot_State_t g_robot_state; extern Remote_t g_remote; extern Referee_System_t Referee_System; +extern DJI_Motor_Handle_t *g_yaw; DJI_Motor_Handle_t *g_azimuth_motors[NUMBER_OF_MODULES]; DJI_Motor_Handle_t *g_drive_motors[NUMBER_OF_MODULES]; @@ -42,10 +43,10 @@ void Chassis_Task_Init() }, .velocity_pid = { - .kp = 300.0f, + .kp = 100.0f, .ki = 0.0f, - .kd = 100.0f, - .kf = 2000.0f, + .kd = 0.0f, + .kf = 000.0f, .feedforward_limit = 5000.0f, .integral_limit = 5000.0f, .output_limit = GM6020_MAX_VOLTAGE_INT, @@ -110,7 +111,7 @@ void Chassis_Task_Init() rate_limiter_init(&chassis_omega_limiter, SWERVE_MAX_OMEGA_ACCEL); // Initialize PID for locking - PID_Init(&angle_locking_pid, 20, 0, 2500, 2 * PI * 30, 0, 0); + PID_Init(&angle_locking_pid, 1, 0, 0, 2 * PI * 1, 0, 0.05); } void Chassis_Ctrl_Loop() @@ -154,38 +155,50 @@ void Chassis_Ctrl_Loop() // g_chassis_state.v_y = g_chassis_state.v_x * sin(theta) + g_chassis_state.v_y * cos(theta); // If spintop enabled, chassis omega set to spintop value - gimbal_angle_difference = g_robot_state.gimbal.yaw_angle; - float chassis_omega_new_target; + // gimbal_angle_difference = g_robot_state.gimbal.yaw_angle; + // float chassis_omega_new_target; + // if (g_robot_state.chassis.IS_SPINTOP_ENABLED) { + // g_chassis_state.omega = rate_limiter_iterate(&chassis_omega_limiter, Rescale_Chassis_Velocity()); + // } + // else if (g_robot_state.chassis.locked_state == STRAIGHT) { + // __MAP_ANGLE_TO_UNIT_CIRCLE(gimbal_angle_difference); + + // gimbal_angle_difference = __MOD_F(gimbal_angle_difference, PI / 2); + // if (gimbal_angle_difference > PI / 4) { // might be easier to go by the way of spin top direction + // gimbal_angle_difference = -1 * ((PI / 2) - gimbal_angle_difference); + // } + // chassis_omega_new_target = PID(&angle_locking_pid, -gimbal_angle_difference); + // __MAX_LIMIT(chassis_omega_new_target, -1 * g_spintop_omega, g_spintop_omega); // might not need this + // g_chassis_state.omega = rate_limiter_iterate(&chassis_omega_limiter, chassis_omega_new_target); + // } + // else if (g_robot_state.chassis.locked_state == ANGLED) { + // __MAP_ANGLE_TO_UNIT_CIRCLE(gimbal_angle_difference); + + // gimbal_angle_difference += PI / 4; + // gimbal_angle_difference = __MOD_F(gimbal_angle_difference, PI / 2); + // if (gimbal_angle_difference > PI / 4) { + // gimbal_angle_difference = -1 * ((PI / 2) - gimbal_angle_difference); + // } + // chassis_omega_new_target = PID(&angle_locking_pid, -gimbal_angle_difference); + // __MAX_LIMIT(chassis_omega_new_target, -1 * g_spintop_omega, g_spintop_omega); + // g_chassis_state.omega = rate_limiter_iterate(&chassis_omega_limiter, chassis_omega_new_target); + // } + // else { // random locking + // g_chassis_state.omega = rate_limiter_iterate(&chassis_omega_limiter, 0); + // } + float tmpgimbal_angle_difference = DJI_Motor_Get_Absolute_Angle(g_yaw); + //float chassis_omega_new_target; + + // if (g_remote.controller.left_switch ) if (g_robot_state.chassis.IS_SPINTOP_ENABLED) { g_chassis_state.omega = rate_limiter_iterate(&chassis_omega_limiter, Rescale_Chassis_Velocity()); - } - else if (g_robot_state.chassis.locked_state == STRAIGHT) { - __MAP_ANGLE_TO_UNIT_CIRCLE(gimbal_angle_difference); + } else { + __MAP_ANGLE_TO_UNIT_CIRCLE(tmpgimbal_angle_difference); + gimbal_angle_difference = tmpgimbal_angle_difference; + g_chassis_state.omega = PID(&angle_locking_pid, gimbal_angle_difference); - gimbal_angle_difference = __MOD_F(gimbal_angle_difference, PI / 2); - if (gimbal_angle_difference > PI / 4) { // might be easier to go by the way of spin top direction - gimbal_angle_difference = -1 * ((PI / 2) - gimbal_angle_difference); - } - chassis_omega_new_target = PID(&angle_locking_pid, gimbal_angle_difference); - __MAX_LIMIT(chassis_omega_new_target, -1 * g_spintop_omega, g_spintop_omega); // might not need this - g_chassis_state.omega = rate_limiter_iterate(&chassis_omega_limiter, chassis_omega_new_target); - } - else if (g_robot_state.chassis.locked_state == ANGLED) { - __MAP_ANGLE_TO_UNIT_CIRCLE(gimbal_angle_difference); - - gimbal_angle_difference += PI / 4; - gimbal_angle_difference = __MOD_F(gimbal_angle_difference, PI / 2); - if (gimbal_angle_difference > PI / 4) { - gimbal_angle_difference = -1 * ((PI / 2) - gimbal_angle_difference); - } - chassis_omega_new_target = PID(&angle_locking_pid, gimbal_angle_difference); - __MAX_LIMIT(chassis_omega_new_target, -1 * g_spintop_omega, g_spintop_omega); - g_chassis_state.omega = rate_limiter_iterate(&chassis_omega_limiter, chassis_omega_new_target); - } - else { // random locking - g_chassis_state.omega = rate_limiter_iterate(&chassis_omega_limiter, 0); } - + // Calculate the kinematics of the chassis swerve_calculate_kinematics(&g_chassis_state, &g_swerve_constants); swerve_optimize_module_angles(&g_chassis_state, measured_angles); diff --git a/Swerve-Standard/src/robot.c b/Swerve-Standard/src/robot.c index 58c1eb7..19874f9 100644 --- a/Swerve-Standard/src/robot.c +++ b/Swerve-Standard/src/robot.c @@ -133,6 +133,7 @@ void Process_Remote_Input() { g_robot_state.IS_SUPER_CAPACITOR_ENABLED = 1; } else { + g_robot_state.IS_SUPER_CAPACITOR_ENABLED = 0; } @@ -173,11 +174,20 @@ void Process_Remote_Input() //g_robtot_state.launch.IS_FIRING_ENABLED = 1; g_robot_state.launch.IS_AUTO_AIMING_ENABLED = 1; } else { - //g_robot_state.chassis.IS_SPINTOP_ENABLED = 0; + g_robot_state.chassis.IS_SPINTOP_ENABLED = 0; //g_robot_state.launch.IS_FIRING_ENABLED = 0; g_robot_state.launch.IS_AUTO_AIMING_ENABLED = 0; } + if (g_remote.controller.left_switch == MID) + { + g_robot_state.chassis.IS_SPINTOP_ENABLED = 1; + } + else + { + g_robot_state.chassis.IS_SPINTOP_ENABLED = 0; + } + // Update previous states keyboard g_input_state.prev_B = g_remote.keyboard.B; g_input_state.prev_G = g_remote.keyboard.G;