diff --git a/src/EZ-Template/drive/set_pid/set_swing_pid.cpp b/src/EZ-Template/drive/set_pid/set_swing_pid.cpp index a5826d52..41d2b787 100644 --- a/src/EZ-Template/drive/set_pid/set_swing_pid.cpp +++ b/src/EZ-Template/drive/set_pid/set_swing_pid.cpp @@ -285,11 +285,16 @@ void Drive::pid_swing_set(e_swing type, double target, int speed, int opposite_s // Print targets if (print_toggle) printf("Swing Started... Target Value: %.2f\n", target); - current_swing = type; + chain_sensor_start = drive_imu_get(); chain_target_start = target; used_motion_chain_scale = 0.0; + // Flip the swing from left-right if rotation axis is flipped + if (odom_theta_direction_get()) + current_swing = type == ez::LEFT_SWING ? ez::RIGHT_SWING : ez::LEFT_SWING; + + // Figure out if going forward or backward int side = type == ez::LEFT_SWING ? 1 : -1; int direction = util::sgn((target - chain_sensor_start) * side);