[wpilib] DifferentialDrive: Remove right side inversion (#3340)

Also refactor drive inverse kinematics into separate functions.
This allows composing them with operations separate from the drive
class.

wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java

@@ -83,32 +83,36 @@ import java.util.StringJoiner;
  * <p>Inputs smaller then {@value edu.wpi.first.wpilibj.drive.RobotDriveBase#kDefaultDeadband} will
  * be set to 0, and larger values will be scaled so that the full range is still used. This deadband
  * value can be changed with {@link #setDeadband}.
- *
- * <p>RobotDrive porting guide: <br>
- * {@link #tankDrive(double, double)} is equivalent to RobotDrive's tankDrive(double, double) if a
- * deadband of 0 is used. <br>
- * {@link #arcadeDrive(double, double)} is equivalent to RobotDrive's arcadeDrive(double, double) if
- * a deadband of 0 is used and the the rotation input is inverted eg arcadeDrive(y, -rotation) <br>
- * {@link #curvatureDrive(double, double, boolean)} is similar in concept to RobotDrive's
- * drive(double, double) with the addition of a quick turn mode. However, it is not designed to give
- * exactly the same response.
  */
 @SuppressWarnings("removal")
 public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoCloseable {
-  public static final double kDefaultQuickStopThreshold = 0.2;
-  public static final double kDefaultQuickStopAlpha = 0.1;
-
   private static int instances;
 
   private final SpeedController m_leftMotor;
   private final SpeedController m_rightMotor;
 
-  private double m_quickStopThreshold = kDefaultQuickStopThreshold;
-  private double m_quickStopAlpha = kDefaultQuickStopAlpha;
-  private double m_quickStopAccumulator;
-  private double m_rightSideInvertMultiplier = -1.0;
   private boolean m_reported;
 
+  @SuppressWarnings("MemberName")
+  public static class WheelSpeeds {
+    public double left;
+    public double right;
+
+    /** Constructs a WheelSpeeds with zeroes for left and right speeds. */
+    public WheelSpeeds() {}
+
+    /**
+     * Constructs a WheelSpeeds.
+     *
+     * @param left The left speed.
+     * @param right The right speed.
+     */
+    public WheelSpeeds(double left, double right) {
+      this.left = left;
+      this.right = right;
+    }
+  }
+
   /**
    * Construct a DifferentialDrive.
    *
@@ -183,47 +187,13 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC
       m_reported = true;
     }
 
-    xSpeed = MathUtil.clamp(xSpeed, -1.0, 1.0);
     xSpeed = applyDeadband(xSpeed, m_deadband);
-
-    zRotation = MathUtil.clamp(zRotation, -1.0, 1.0);
     zRotation = applyDeadband(zRotation, m_deadband);
 
-    // Square the inputs (while preserving the sign) to increase fine control
-    // while permitting full power.
-    if (squareInputs) {
-      xSpeed = Math.copySign(xSpeed * xSpeed, xSpeed);
-      zRotation = Math.copySign(zRotation * zRotation, zRotation);
-    }
+    var speeds = arcadeDriveIK(xSpeed, zRotation, squareInputs);
 
-    double leftMotorOutput;
-    double rightMotorOutput;
-
-    double maxInput = Math.copySign(Math.max(Math.abs(xSpeed), Math.abs(zRotation)), xSpeed);
-
-    if (xSpeed >= 0.0) {
-      // First quadrant, else second quadrant
-      if (zRotation >= 0.0) {
-        leftMotorOutput = maxInput;
-        rightMotorOutput = xSpeed - zRotation;
-      } else {
-        leftMotorOutput = xSpeed + zRotation;
-        rightMotorOutput = maxInput;
-      }
-    } else {
-      // Third quadrant, else fourth quadrant
-      if (zRotation >= 0.0) {
-        leftMotorOutput = xSpeed + zRotation;
-        rightMotorOutput = maxInput;
-      } else {
-        leftMotorOutput = maxInput;
-        rightMotorOutput = xSpeed - zRotation;
-      }
-    }
-
-    m_leftMotor.set(MathUtil.clamp(leftMotorOutput, -1.0, 1.0) * m_maxOutput);
-    double maxOutput = m_maxOutput * m_rightSideInvertMultiplier;
-    m_rightMotor.set(MathUtil.clamp(rightMotorOutput, -1.0, 1.0) * maxOutput);
+    m_leftMotor.set(speeds.left * m_maxOutput);
+    m_rightMotor.set(speeds.right * m_maxOutput);
 
     feed();
   }
@@ -239,75 +209,24 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC
    * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
    * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
    *     positive.
-   * @param isQuickTurn If set, overrides constant-curvature turning for turn-in-place maneuvers.
+   * @param allowTurnInPlace If set, overrides constant-curvature turning for turn-in-place
+   *     maneuvers.
    */
   @SuppressWarnings({"ParameterName", "PMD.CyclomaticComplexity"})
-  public void curvatureDrive(double xSpeed, double zRotation, boolean isQuickTurn) {
+  public void curvatureDrive(double xSpeed, double zRotation, boolean allowTurnInPlace) {
     if (!m_reported) {
       HAL.report(
           tResourceType.kResourceType_RobotDrive, tInstances.kRobotDrive2_DifferentialCurvature, 2);
       m_reported = true;
     }
 
-    xSpeed = MathUtil.clamp(xSpeed, -1.0, 1.0);
     xSpeed = applyDeadband(xSpeed, m_deadband);
-
-    zRotation = MathUtil.clamp(zRotation, -1.0, 1.0);
     zRotation = applyDeadband(zRotation, m_deadband);
 
-    double angularPower;
-    boolean overPower;
+    var speeds = curvatureDriveIK(xSpeed, zRotation, allowTurnInPlace);
 
-    if (isQuickTurn) {
-      if (Math.abs(xSpeed) < m_quickStopThreshold) {
-        m_quickStopAccumulator =
-            (1 - m_quickStopAlpha) * m_quickStopAccumulator
-                + m_quickStopAlpha * MathUtil.clamp(zRotation, -1.0, 1.0) * 2;
-      }
-      overPower = true;
-      angularPower = zRotation;
-    } else {
-      overPower = false;
-      angularPower = Math.abs(xSpeed) * zRotation - m_quickStopAccumulator;
-
-      if (m_quickStopAccumulator > 1) {
-        m_quickStopAccumulator -= 1;
-      } else if (m_quickStopAccumulator < -1) {
-        m_quickStopAccumulator += 1;
-      } else {
-        m_quickStopAccumulator = 0.0;
-      }
-    }
-
-    double leftMotorOutput = xSpeed + angularPower;
-    double rightMotorOutput = xSpeed - angularPower;
-
-    // If rotation is overpowered, reduce both outputs to within acceptable range
-    if (overPower) {
-      if (leftMotorOutput > 1.0) {
-        rightMotorOutput -= leftMotorOutput - 1.0;
-        leftMotorOutput = 1.0;
-      } else if (rightMotorOutput > 1.0) {
-        leftMotorOutput -= rightMotorOutput - 1.0;
-        rightMotorOutput = 1.0;
-      } else if (leftMotorOutput < -1.0) {
-        rightMotorOutput -= leftMotorOutput + 1.0;
-        leftMotorOutput = -1.0;
-      } else if (rightMotorOutput < -1.0) {
-        leftMotorOutput -= rightMotorOutput + 1.0;
-        rightMotorOutput = -1.0;
-      }
-    }
-
-    // Normalize the wheel speeds
-    double maxMagnitude = Math.max(Math.abs(leftMotorOutput), Math.abs(rightMotorOutput));
-    if (maxMagnitude > 1.0) {
-      leftMotorOutput /= maxMagnitude;
-      rightMotorOutput /= maxMagnitude;
-    }
-
-    m_leftMotor.set(leftMotorOutput * m_maxOutput);
-    m_rightMotor.set(rightMotorOutput * m_maxOutput * m_rightSideInvertMultiplier);
+    m_leftMotor.set(speeds.left * m_maxOutput);
+    m_rightMotor.set(speeds.right * m_maxOutput);
 
     feed();
   }
@@ -339,12 +258,125 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC
       m_reported = true;
     }
 
-    leftSpeed = MathUtil.clamp(leftSpeed, -1.0, 1.0);
     leftSpeed = applyDeadband(leftSpeed, m_deadband);
-
-    rightSpeed = MathUtil.clamp(rightSpeed, -1.0, 1.0);
     rightSpeed = applyDeadband(rightSpeed, m_deadband);
 
+    var speeds = tankDriveIK(leftSpeed, rightSpeed, squareInputs);
+
+    m_leftMotor.set(speeds.left * m_maxOutput);
+    m_rightMotor.set(speeds.right * m_maxOutput);
+
+    feed();
+  }
+
+  /**
+   * Arcade drive inverse kinematics for differential drive platform.
+   *
+   * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
+   * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
+   *     positive.
+   * @param squareInputs If set, decreases the input sensitivity at low speeds.
+   */
+  @SuppressWarnings("ParameterName")
+  public static WheelSpeeds arcadeDriveIK(double xSpeed, double zRotation, boolean squareInputs) {
+    xSpeed = MathUtil.clamp(xSpeed, -1.0, 1.0);
+    zRotation = MathUtil.clamp(zRotation, -1.0, 1.0);
+
+    // Square the inputs (while preserving the sign) to increase fine control
+    // while permitting full power.
+    if (squareInputs) {
+      xSpeed = Math.copySign(xSpeed * xSpeed, xSpeed);
+      zRotation = Math.copySign(zRotation * zRotation, zRotation);
+    }
+
+    double leftSpeed;
+    double rightSpeed;
+
+    double maxInput = Math.copySign(Math.max(Math.abs(xSpeed), Math.abs(zRotation)), xSpeed);
+
+    if (xSpeed >= 0.0) {
+      // First quadrant, else second quadrant
+      if (zRotation >= 0.0) {
+        leftSpeed = maxInput;
+        rightSpeed = xSpeed - zRotation;
+      } else {
+        leftSpeed = xSpeed + zRotation;
+        rightSpeed = maxInput;
+      }
+    } else {
+      // Third quadrant, else fourth quadrant
+      if (zRotation >= 0.0) {
+        leftSpeed = xSpeed + zRotation;
+        rightSpeed = maxInput;
+      } else {
+        leftSpeed = maxInput;
+        rightSpeed = xSpeed - zRotation;
+      }
+    }
+
+    // Normalize the wheel speeds
+    double maxMagnitude = Math.max(Math.abs(leftSpeed), Math.abs(rightSpeed));
+    if (maxMagnitude > 1.0) {
+      leftSpeed /= maxMagnitude;
+      rightSpeed /= maxMagnitude;
+    }
+
+    return new WheelSpeeds(leftSpeed, rightSpeed);
+  }
+
+  /**
+   * Curvature drive inverse kinematics for differential drive platform.
+   *
+   * <p>The rotation argument controls the curvature of the robot's path rather than its rate of
+   * heading change. This makes the robot more controllable at high speeds. Also handles the robot's
+   * quick turn functionality - "quick turn" overrides constant-curvature turning for turn-in-place
+   * maneuvers.
+   *
+   * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
+   * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
+   *     positive.
+   * @param allowTurnInPlace If set, overrides constant-curvature turning for turn-in-place
+   *     maneuvers.
+   */
+  @SuppressWarnings("ParameterName")
+  public static WheelSpeeds curvatureDriveIK(
+      double xSpeed, double zRotation, boolean allowTurnInPlace) {
+    xSpeed = MathUtil.clamp(xSpeed, -1.0, 1.0);
+    zRotation = MathUtil.clamp(zRotation, -1.0, 1.0);
+
+    double leftSpeed = 0.0;
+    double rightSpeed = 0.0;
+
+    if (allowTurnInPlace) {
+      leftSpeed = xSpeed + zRotation;
+      rightSpeed = xSpeed - zRotation;
+    } else {
+      leftSpeed = xSpeed + Math.abs(xSpeed) * zRotation;
+      rightSpeed = xSpeed - Math.abs(xSpeed) * zRotation;
+    }
+
+    // Normalize wheel speeds
+    double maxMagnitude = Math.max(Math.abs(leftSpeed), Math.abs(rightSpeed));
+    if (maxMagnitude > 1.0) {
+      leftSpeed /= maxMagnitude;
+      rightSpeed /= maxMagnitude;
+    }
+
+    return new WheelSpeeds(leftSpeed, rightSpeed);
+  }
+
+  /**
+   * Tank drive inverse kinematics for differential drive platform.
+   *
+   * @param leftSpeed The robot left side's speed along the X axis [-1.0..1.0]. Forward is positive.
+   * @param rightSpeed The robot right side's speed along the X axis [-1.0..1.0]. Forward is
+   *     positive.
+   * @param squareInputs If set, decreases the input sensitivity at low speeds.
+   */
+  public static WheelSpeeds tankDriveIK(double leftSpeed, double rightSpeed, boolean squareInputs) {
+    leftSpeed = MathUtil.clamp(leftSpeed, -1.0, 1.0);
+    rightSpeed = MathUtil.clamp(rightSpeed, -1.0, 1.0);
+
     // Square the inputs (while preserving the sign) to increase fine control
     // while permitting full power.
     if (squareInputs) {
@@ -352,59 +384,7 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC
       rightSpeed = Math.copySign(rightSpeed * rightSpeed, rightSpeed);
     }
 
-    m_leftMotor.set(leftSpeed * m_maxOutput);
-    m_rightMotor.set(rightSpeed * m_maxOutput * m_rightSideInvertMultiplier);
-
-    feed();
-  }
-
-  /**
-   * Sets the QuickStop speed threshold in curvature drive.
-   *
-   * <p>QuickStop compensates for the robot's moment of inertia when stopping after a QuickTurn.
-   *
-   * <p>While QuickTurn is enabled, the QuickStop accumulator takes on the rotation rate value
-   * outputted by the low-pass filter when the robot's speed along the X axis is below the
-   * threshold. When QuickTurn is disabled, the accumulator's value is applied against the computed
-   * angular power request to slow the robot's rotation.
-   *
-   * @param threshold X speed below which quick stop accumulator will receive rotation rate values
-   *     [0..1.0].
-   */
-  public void setQuickStopThreshold(double threshold) {
-    m_quickStopThreshold = threshold;
-  }
-
-  /**
-   * Sets the low-pass filter gain for QuickStop in curvature drive.
-   *
-   * <p>The low-pass filter filters incoming rotation rate commands to smooth out high frequency
-   * changes.
-   *
-   * @param alpha Low-pass filter gain [0.0..2.0]. Smaller values result in slower output changes.
-   *     Values between 1.0 and 2.0 result in output oscillation. Values below 0.0 and above 2.0 are
-   *     unstable.
-   */
-  public void setQuickStopAlpha(double alpha) {
-    m_quickStopAlpha = alpha;
-  }
-
-  /**
-   * Gets if the power sent to the right side of the drivetrain is multiplied by -1.
-   *
-   * @return true if the right side is inverted
-   */
-  public boolean isRightSideInverted() {
-    return m_rightSideInvertMultiplier == -1.0;
-  }
-
-  /**
-   * Sets if the power sent to the right side of the drivetrain should be multiplied by -1.
-   *
-   * @param rightSideInverted true if right side power should be multiplied by -1
-   */
-  public void setRightSideInverted(boolean rightSideInverted) {
-    m_rightSideInvertMultiplier = rightSideInverted ? -1.0 : 1.0;
+    return new WheelSpeeds(leftSpeed, rightSpeed);
   }
 
   @Override
@@ -426,8 +406,6 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC
     builder.setSafeState(this::stopMotor);
     builder.addDoubleProperty("Left Motor Speed", m_leftMotor::get, m_leftMotor::set);
     builder.addDoubleProperty(
-        "Right Motor Speed",
-        () -> m_rightMotor.get() * m_rightSideInvertMultiplier,
-        x -> m_rightMotor.set(x * m_rightSideInvertMultiplier));
+        "Right Motor Speed", () -> m_rightMotor.get(), x -> m_rightMotor.set(x));
   }
 }

wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/KilloughDrive.java

@@ -57,6 +57,29 @@ public class KilloughDrive extends RobotDriveBase implements Sendable, AutoClose
 
   private boolean m_reported;
 
+  @SuppressWarnings("MemberName")
+  public static class WheelSpeeds {
+    public double left;
+    public double right;
+    public double back;
+
+    /** Constructs a WheelSpeeds with zeroes for left, right, and back speeds. */
+    public WheelSpeeds() {}
+
+    /**
+     * Constructs a WheelSpeeds.
+     *
+     * @param left The left speed.
+     * @param right The right speed.
+     * @param back The back speed.
+     */
+    public WheelSpeeds(double left, double right, double back) {
+      this.left = left;
+      this.right = right;
+      this.back = back;
+    }
+  }
+
   /**
    * Construct a Killough drive with the given motors and default motor angles.
    *
@@ -191,26 +214,14 @@ public class KilloughDrive extends RobotDriveBase implements Sendable, AutoClose
       m_reported = true;
     }
 
-    ySpeed = MathUtil.clamp(ySpeed, -1.0, 1.0);
     ySpeed = applyDeadband(ySpeed, m_deadband);
-
-    xSpeed = MathUtil.clamp(xSpeed, -1.0, 1.0);
     xSpeed = applyDeadband(xSpeed, m_deadband);
 
-    // Compensate for gyro angle.
-    Vector2d input = new Vector2d(ySpeed, xSpeed);
-    input.rotate(-gyroAngle);
+    var speeds = driveCartesianIK(ySpeed, xSpeed, zRotation, gyroAngle);
 
-    double[] wheelSpeeds = new double[3];
-    wheelSpeeds[MotorType.kLeft.value] = input.scalarProject(m_leftVec) + zRotation;
-    wheelSpeeds[MotorType.kRight.value] = input.scalarProject(m_rightVec) + zRotation;
-    wheelSpeeds[MotorType.kBack.value] = input.scalarProject(m_backVec) + zRotation;
-
-    normalize(wheelSpeeds);
-
-    m_leftMotor.set(wheelSpeeds[MotorType.kLeft.value] * m_maxOutput);
-    m_rightMotor.set(wheelSpeeds[MotorType.kRight.value] * m_maxOutput);
-    m_backMotor.set(wheelSpeeds[MotorType.kBack.value] * m_maxOutput);
+    m_leftMotor.set(speeds.left * m_maxOutput);
+    m_rightMotor.set(speeds.right * m_maxOutput);
+    m_backMotor.set(speeds.back * m_maxOutput);
 
     feed();
   }
@@ -240,6 +251,42 @@ public class KilloughDrive extends RobotDriveBase implements Sendable, AutoClose
         0.0);
   }
 
+  /**
+   * Cartesian inverse kinematics for Killough platform.
+   *
+   * <p>Angles are measured clockwise from the positive X axis. The robot's speed is independent
+   * from its angle or rotation rate.
+   *
+   * @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Right is positive.
+   * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
+   * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
+   *     positive.
+   * @param gyroAngle The current angle reading from the gyro in degrees around the Z axis. Use this
+   *     to implement field-oriented controls.
+   */
+  @SuppressWarnings("ParameterName")
+  public WheelSpeeds driveCartesianIK(
+      double ySpeed, double xSpeed, double zRotation, double gyroAngle) {
+    ySpeed = MathUtil.clamp(ySpeed, -1.0, 1.0);
+    xSpeed = MathUtil.clamp(xSpeed, -1.0, 1.0);
+
+    // Compensate for gyro angle.
+    Vector2d input = new Vector2d(ySpeed, xSpeed);
+    input.rotate(-gyroAngle);
+
+    double[] wheelSpeeds = new double[3];
+    wheelSpeeds[MotorType.kLeft.value] = input.scalarProject(m_leftVec) + zRotation;
+    wheelSpeeds[MotorType.kRight.value] = input.scalarProject(m_rightVec) + zRotation;
+    wheelSpeeds[MotorType.kBack.value] = input.scalarProject(m_backVec) + zRotation;
+
+    normalize(wheelSpeeds);
+
+    return new WheelSpeeds(
+        wheelSpeeds[MotorType.kLeft.value],
+        wheelSpeeds[MotorType.kRight.value],
+        wheelSpeeds[MotorType.kBack.value]);
+  }
+
   @Override
   public void stopMotor() {
     m_leftMotor.stopMotor();

wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java

@@ -66,9 +66,34 @@ public class MecanumDrive extends RobotDriveBase implements Sendable, AutoClosea
   private final SpeedController m_frontRightMotor;
   private final SpeedController m_rearRightMotor;
 
-  private double m_rightSideInvertMultiplier = -1.0;
   private boolean m_reported;
 
+  @SuppressWarnings("MemberName")
+  public static class WheelSpeeds {
+    public double frontLeft;
+    public double frontRight;
+    public double rearLeft;
+    public double rearRight;
+
+    /** Constructs a WheelSpeeds with zeroes for all four speeds. */
+    public WheelSpeeds() {}
+
+    /**
+     * Constructs a WheelSpeeds.
+     *
+     * @param frontLeft The front left speed.
+     * @param frontRight The front right speed.
+     * @param rearLeft The rear left speed.
+     * @param rearRight The rear right speed.
+     */
+    public WheelSpeeds(double frontLeft, double frontRight, double rearLeft, double rearRight) {
+      this.frontLeft = frontLeft;
+      this.frontRight = frontRight;
+      this.rearLeft = rearLeft;
+      this.rearRight = rearRight;
+    }
+  }
+
   /**
    * Construct a MecanumDrive.
    *
@@ -167,30 +192,15 @@ public class MecanumDrive extends RobotDriveBase implements Sendable, AutoClosea
       m_reported = true;
     }
 
-    ySpeed = MathUtil.clamp(ySpeed, -1.0, 1.0);
     ySpeed = applyDeadband(ySpeed, m_deadband);
-
-    xSpeed = MathUtil.clamp(xSpeed, -1.0, 1.0);
     xSpeed = applyDeadband(xSpeed, m_deadband);
 
-    // Compensate for gyro angle.
-    Vector2d input = new Vector2d(ySpeed, xSpeed);
-    input.rotate(-gyroAngle);
+    var speeds = driveCartesianIK(ySpeed, xSpeed, zRotation, gyroAngle);
 
-    double[] wheelSpeeds = new double[4];
-    wheelSpeeds[MotorType.kFrontLeft.value] = input.x + input.y + zRotation;
-    wheelSpeeds[MotorType.kFrontRight.value] = -input.x + input.y - zRotation;
-    wheelSpeeds[MotorType.kRearLeft.value] = -input.x + input.y + zRotation;
-    wheelSpeeds[MotorType.kRearRight.value] = input.x + input.y - zRotation;
-
-    normalize(wheelSpeeds);
-
-    m_frontLeftMotor.set(wheelSpeeds[MotorType.kFrontLeft.value] * m_maxOutput);
-    m_frontRightMotor.set(
-        wheelSpeeds[MotorType.kFrontRight.value] * m_maxOutput * m_rightSideInvertMultiplier);
-    m_rearLeftMotor.set(wheelSpeeds[MotorType.kRearLeft.value] * m_maxOutput);
-    m_rearRightMotor.set(
-        wheelSpeeds[MotorType.kRearRight.value] * m_maxOutput * m_rightSideInvertMultiplier);
+    m_frontLeftMotor.set(speeds.frontLeft * m_maxOutput);
+    m_frontRightMotor.set(speeds.frontRight * m_maxOutput);
+    m_rearLeftMotor.set(speeds.rearLeft * m_maxOutput);
+    m_rearRightMotor.set(speeds.rearRight * m_maxOutput);
 
     feed();
   }
@@ -214,28 +224,48 @@ public class MecanumDrive extends RobotDriveBase implements Sendable, AutoClosea
     }
 
     driveCartesian(
-        magnitude * Math.sin(angle * (Math.PI / 180.0)),
         magnitude * Math.cos(angle * (Math.PI / 180.0)),
+        magnitude * Math.sin(angle * (Math.PI / 180.0)),
         zRotation,
         0.0);
   }
 
   /**
-   * Gets if the power sent to the right side of the drivetrain is multiplied by -1.
+   * Cartesian inverse kinematics for Mecanum platform.
    *
-   * @return true if the right side is inverted
+   * <p>Angles are measured clockwise from the positive X axis. The robot's speed is independent
+   * from its angle or rotation rate.
+   *
+   * @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Right is positive.
+   * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
+   * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
+   *     positive.
+   * @param gyroAngle The current angle reading from the gyro in degrees around the Z axis. Use this
+   *     to implement field-oriented controls.
    */
-  public boolean isRightSideInverted() {
-    return m_rightSideInvertMultiplier == -1.0;
-  }
+  @SuppressWarnings("ParameterName")
+  public static WheelSpeeds driveCartesianIK(
+      double ySpeed, double xSpeed, double zRotation, double gyroAngle) {
+    ySpeed = MathUtil.clamp(ySpeed, -1.0, 1.0);
+    xSpeed = MathUtil.clamp(xSpeed, -1.0, 1.0);
 
-  /**
-   * Sets if the power sent to the right side of the drivetrain should be multiplied by -1.
-   *
-   * @param rightSideInverted true if right side power should be multiplied by -1
-   */
-  public void setRightSideInverted(boolean rightSideInverted) {
-    m_rightSideInvertMultiplier = rightSideInverted ? -1.0 : 1.0;
+    // Compensate for gyro angle.
+    Vector2d input = new Vector2d(ySpeed, xSpeed);
+    input.rotate(-gyroAngle);
+
+    double[] wheelSpeeds = new double[4];
+    wheelSpeeds[MotorType.kFrontLeft.value] = input.x + input.y + zRotation;
+    wheelSpeeds[MotorType.kFrontRight.value] = input.x - input.y - zRotation;
+    wheelSpeeds[MotorType.kRearLeft.value] = input.x - input.y + zRotation;
+    wheelSpeeds[MotorType.kRearRight.value] = input.x + input.y - zRotation;
+
+    normalize(wheelSpeeds);
+
+    return new WheelSpeeds(
+        wheelSpeeds[MotorType.kFrontLeft.value],
+        wheelSpeeds[MotorType.kFrontRight.value],
+        wheelSpeeds[MotorType.kRearLeft.value],
+        wheelSpeeds[MotorType.kRearRight.value]);
   }
 
   @Override
@@ -261,12 +291,12 @@ public class MecanumDrive extends RobotDriveBase implements Sendable, AutoClosea
         "Front Left Motor Speed", m_frontLeftMotor::get, m_frontLeftMotor::set);
     builder.addDoubleProperty(
         "Front Right Motor Speed",
-        () -> m_frontRightMotor.get() * m_rightSideInvertMultiplier,
-        value -> m_frontRightMotor.set(value * m_rightSideInvertMultiplier));
+        () -> m_frontRightMotor.get(),
+        value -> m_frontRightMotor.set(value));
     builder.addDoubleProperty("Rear Left Motor Speed", m_rearLeftMotor::get, m_rearLeftMotor::set);
     builder.addDoubleProperty(
         "Rear Right Motor Speed",
-        () -> m_rearRightMotor.get() * m_rightSideInvertMultiplier,
-        value -> m_rearRightMotor.set(value * m_rightSideInvertMultiplier));
+        () -> m_rearRightMotor.get(),
+        value -> m_rearRightMotor.set(value));
   }
 }

wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/RobotDriveBase.java

@@ -83,7 +83,7 @@ public abstract class RobotDriveBase extends MotorSafety {
    * @param value value to clip
    * @param deadband range around zero
    */
-  protected double applyDeadband(double value, double deadband) {
+  protected static double applyDeadband(double value, double deadband) {
     if (Math.abs(value) > deadband) {
       if (value > 0.0) {
         return (value - deadband) / (1.0 - deadband);
@@ -96,7 +96,7 @@ public abstract class RobotDriveBase extends MotorSafety {
   }
 
   /** Normalize all wheel speeds if the magnitude of any wheel is greater than 1.0. */
-  protected void normalize(double[] wheelSpeeds) {
+  protected static void normalize(double[] wheelSpeeds) {
     double maxMagnitude = Math.abs(wheelSpeeds[0]);
     for (int i = 1; i < wheelSpeeds.length; i++) {
       double temp = Math.abs(wheelSpeeds[i]);