# Field-Centric swerve drive

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));

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!

WPILib 2020 has SwerveDriveKinematics built in. There’s also examples.

WPILib’s coordinate system is NWU (x forward, y left, z up), which means angles are CCW positive. You need that for functions like \cos(\theta) to return what you expect (the x component of whatever).

https://github.com/wpilibsuite/allwpilib/pull/2248 adds swerve module angle optimization.

3 Likes

Make sure your angles are in radians - that’s what the Java trig functions expect.

1 Like

Thanks, this was it! Works like a charm now