Field-Centric swerve drive


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!

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). adds swerve module angle optimization.


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 :man_facepalming:

Thanks for your reply! I didn’t know these classes existed.

I’ve marked @dydx’s reply as the solution as it fixed this immediate problem, however your suggestion looks like a better solution than rolling our own. I’m guessing it also makes using Pathweaver more seamless? Definitely going to try out your suggestion this week.

This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.