Greetings!
Our team is working on a swerve drive this year and we’ve been using this post from Ether as a reference point for programming it.
Right now we have it driving just fine with +Y always being the direction of the front of the robot, but we’re having trouble getting the calculation to work when using the field-centric example in this PDF.
temp = FWD·cos(θ) + STR·sin(θ);
STR = -FWD·sin(θ) + STR·cos(θ);
FWD = temp;
When these lines are added in, the angle that the robot thinks it’s facing changes seemingly randomly but our control inputs aren’t changing and we’ve confirmed that the gyro drift is only a few hundredths of a degree between each frame. Below is the method we’re using to calculate the wheel speeds and angles. A couple things to consider:
-
Gyro angle is 0°-360° clockwise
-
Output angles are clamped to -180°-180° for each wheel’s PID controller. We had this at 0°-360° before but the results were the same.
-
FWD, STR, and RCW are just -1 to 1 joystick inputs but we observe this behavior even with unchanging inputs like FWD=1, STR=0, RCW=0
public void SwerveDrive(double gyroAngle, double RCW, double STR, double FWD) { // Drive with 2 strafe axes and 1 rotation axis FWD *= -1; // Adjust vectors for gyro angle // double adjustedFWD = (FWD * Math.cos(gyroAngle)) + (STR * Math.sin(gyroAngle)); // STR = (-FWD * Math.sin(gyroAngle)) + (STR * Math.cos(gyroAngle)); // FWD = adjustedFWD; double wheelBase = Wheelbase; double trackWidth = TrackWidth; double R = Math.sqrt(Math.pow(wheelBase, 2) + Math.pow(trackWidth, 2)); double A = STR - RCW * (wheelBase / R); double B = STR + RCW * (wheelBase / R); double C = FWD - RCW * (trackWidth / R); double D = FWD + RCW * (trackWidth / R); // Front right double frontRightSpeed = Math.sqrt(Math.pow(B, 2) + Math.pow(C, 2)); double frontRightAngle = Math.atan2(B, C) * 180/Math.PI; // Front left double frontLeftSpeed = Math.sqrt(Math.pow(B, 2) + Math.pow(D, 2)); double frontLeftAngle = Math.atan2(B, D) * 180/Math.PI; // Rear right double rearRightSpeed = Math.sqrt(Math.pow(A, 2) + Math.pow(C, 2)); double rearRightAngle = Math.atan2(A, C) * 180/Math.PI; // Rear left double rearLeftSpeed = Math.sqrt(Math.pow(A, 2) + Math.pow(D, 2)); double rearLeftAngle = Math.atan2(A, D) * 180/Math.PI; // If, after calculating the 4 wheel speeds, any of them is greater than 1, then divide all the wheel speeds by the largest value. if (frontRightSpeed > 1 || frontLeftSpeed > 1 || rearRightSpeed > 1 || rearLeftSpeed > 1) { Double[] speeds = new Double[] { frontRightSpeed, frontLeftSpeed, rearRightSpeed, rearLeftSpeed }; double maxValue = Collections.max(Arrays.asList(speeds)); frontRightSpeed /= maxValue; frontLeftSpeed /= maxValue; rearRightSpeed /= maxValue; rearLeftSpeed /= maxValue; } // Calculated angles range from -180° to 180° frontRightAngle = Utilities.ClampAngle(frontRightAngle); frontLeftAngle = Utilities.ClampAngle(frontLeftAngle); rearRightAngle = Utilities.ClampAngle(rearRightAngle); rearLeftAngle = Utilities.ClampAngle(rearLeftAngle); frontRightWheel.SetWheel(frontRightSpeed, frontRightAngle); frontLeftWheel.SetWheel(frontLeftSpeed, frontLeftAngle); rearRightWheel.SetWheel(rearRightSpeed, rearRightAngle); rearLeftWheel.SetWheel(rearLeftSpeed, rearLeftAngle); }
This is our first time doing swerve so any input is greatly appreciated!
