Add setting to invert the right side of the drive (#1045)

wpilibc/src/main/native/cpp/Drive/DifferentialDrive.cpp

@@ -94,7 +94,8 @@ void DifferentialDrive::ArcadeDrive(double xSpeed, double zRotation,
   }
 
   m_leftMotor.Set(Limit(leftMotorOutput) * m_maxOutput);
-  m_rightMotor.Set(-Limit(rightMotorOutput) * m_maxOutput);
+  m_rightMotor.Set(Limit(rightMotorOutput) * m_maxOutput *
+                   m_rightSideInvertMultiplier);
 
   m_safetyHelper.Feed();
 }
@@ -181,7 +182,8 @@ void DifferentialDrive::CurvatureDrive(double xSpeed, double zRotation,
   }
 
   m_leftMotor.Set(leftMotorOutput * m_maxOutput);
-  m_rightMotor.Set(-rightMotorOutput * m_maxOutput);
+  m_rightMotor.Set(rightMotorOutput * m_maxOutput *
+                   m_rightSideInvertMultiplier);
 
   m_safetyHelper.Feed();
 }
@@ -218,7 +220,7 @@ void DifferentialDrive::TankDrive(double leftSpeed, double rightSpeed,
   }
 
   m_leftMotor.Set(leftSpeed * m_maxOutput);
-  m_rightMotor.Set(-rightSpeed * m_maxOutput);
+  m_rightMotor.Set(rightSpeed * m_maxOutput * m_rightSideInvertMultiplier);
 
   m_safetyHelper.Feed();
 }
@@ -256,6 +258,26 @@ void DifferentialDrive::SetQuickStopAlpha(double alpha) {
   m_quickStopAlpha = alpha;
 }
 
+/**
+ * Gets if the power sent to the right side of the drivetrain is multipled by
+ * -1.
+ *
+ * @return true if the right side is inverted
+ */
+bool DifferentialDrive::IsRightSideInverted() const {
+  return m_rightSideInvertMultiplier == -1.0;
+}
+
+/**
+ * Sets if the power sent to the right side of the drivetrain should be
+ * multipled by -1.
+ *
+ * @param rightSideInverted true if right side power should be multipled by -1
+ */
+void DifferentialDrive::SetRightSideInverted(bool rightSideInverted) {
+  m_rightSideInvertMultiplier = rightSideInverted ? -1.0 : 1.0;
+}
+
 void DifferentialDrive::StopMotor() {
   m_leftMotor.StopMotor();
   m_rightMotor.StopMotor();
@@ -271,7 +293,10 @@ void DifferentialDrive::InitSendable(SendableBuilder& builder) {
   builder.AddDoubleProperty("Left Motor Speed",
                             [=]() { return m_leftMotor.Get(); },
                             [=](double value) { m_leftMotor.Set(value); });
-  builder.AddDoubleProperty("Right Motor Speed",
-                            [=]() { return -m_rightMotor.Get(); },
-                            [=](double value) { m_rightMotor.Set(-value); });
+  builder.AddDoubleProperty(
+      "Right Motor Speed",
+      [=]() { return m_rightMotor.Get() * m_rightSideInvertMultiplier; },
+      [=](double value) {
+        m_rightMotor.Set(value * m_rightSideInvertMultiplier);
+      });
 }

wpilibc/src/main/native/cpp/Drive/MecanumDrive.cpp

@@ -77,16 +77,18 @@ void MecanumDrive::DriveCartesian(double ySpeed, double xSpeed,
 
   double wheelSpeeds[4];
   wheelSpeeds[kFrontLeft] = input.x + input.y + zRotation;
-  wheelSpeeds[kFrontRight] = input.x - input.y + zRotation;
+  wheelSpeeds[kFrontRight] = -input.x + input.y - zRotation;
   wheelSpeeds[kRearLeft] = -input.x + input.y + zRotation;
-  wheelSpeeds[kRearRight] = -input.x - input.y + zRotation;
+  wheelSpeeds[kRearRight] = input.x + input.y - zRotation;
 
   Normalize(wheelSpeeds);
 
   m_frontLeftMotor.Set(wheelSpeeds[kFrontLeft] * m_maxOutput);
-  m_frontRightMotor.Set(wheelSpeeds[kFrontRight] * m_maxOutput);
+  m_frontRightMotor.Set(wheelSpeeds[kFrontRight] * m_maxOutput *
+                        m_rightSideInvertMultiplier);
   m_rearLeftMotor.Set(wheelSpeeds[kRearLeft] * m_maxOutput);
-  m_rearRightMotor.Set(wheelSpeeds[kRearRight] * m_maxOutput);
+  m_rearRightMotor.Set(wheelSpeeds[kRearRight] * m_maxOutput *
+                       m_rightSideInvertMultiplier);
 
   m_safetyHelper.Feed();
 }
@@ -116,6 +118,26 @@ void MecanumDrive::DrivePolar(double magnitude, double angle,
                  magnitude * std::cos(angle * (kPi / 180.0)), zRotation, 0.0);
 }
 
+/**
+ * Gets if the power sent to the right side of the drivetrain is multipled by
+ * -1.
+ *
+ * @return true if the right side is inverted
+ */
+bool MecanumDrive::IsRightSideInverted() const {
+  return m_rightSideInvertMultiplier == -1.0;
+}
+
+/**
+ * Sets if the power sent to the right side of the drivetrain should be
+ * multipled by -1.
+ *
+ * @param rightSideInverted true if right side power should be multipled by -1
+ */
+void MecanumDrive::SetRightSideInverted(bool rightSideInverted) {
+  m_rightSideInvertMultiplier = rightSideInverted ? -1.0 : 1.0;
+}
+
 void MecanumDrive::StopMotor() {
   m_frontLeftMotor.StopMotor();
   m_frontRightMotor.StopMotor();
@@ -134,12 +156,18 @@ void MecanumDrive::InitSendable(SendableBuilder& builder) {
                             [=]() { return m_frontLeftMotor.Get(); },
                             [=](double value) { m_frontLeftMotor.Set(value); });
   builder.AddDoubleProperty(
-      "Front Right Motor Speed", [=]() { return -m_frontRightMotor.Get(); },
-      [=](double value) { m_frontRightMotor.Set(-value); });
+      "Front Right Motor Speed",
+      [=]() { return m_frontRightMotor.Get() * m_rightSideInvertMultiplier; },
+      [=](double value) {
+        m_frontRightMotor.Set(value * m_rightSideInvertMultiplier);
+      });
   builder.AddDoubleProperty("Rear Left Motor Speed",
                             [=]() { return m_rearLeftMotor.Get(); },
                             [=](double value) { m_rearLeftMotor.Set(value); });
   builder.AddDoubleProperty(
-      "Rear Right Motor Speed", [=]() { return -m_rearRightMotor.Get(); },
-      [=](double value) { m_rearRightMotor.Set(-value); });
+      "Rear Right Motor Speed",
+      [=]() { return m_rearRightMotor.Get() * m_rightSideInvertMultiplier; },
+      [=](double value) {
+        m_rearRightMotor.Set(value * m_rightSideInvertMultiplier);
+      });
 }

wpilibc/src/main/native/include/Drive/DifferentialDrive.h

@@ -115,6 +115,9 @@ class DifferentialDrive : public RobotDriveBase {
   void SetQuickStopThreshold(double threshold);
   void SetQuickStopAlpha(double alpha);
 
+  bool IsRightSideInverted() const;
+  void SetRightSideInverted(bool rightSideInverted);
+
   void StopMotor() override;
   void GetDescription(wpi::raw_ostream& desc) const override;
 
@@ -127,6 +130,7 @@ class DifferentialDrive : public RobotDriveBase {
   double m_quickStopThreshold = kDefaultQuickStopThreshold;
   double m_quickStopAlpha = kDefaultQuickStopAlpha;
   double m_quickStopAccumulator = 0.0;
+  double m_rightSideInvertMultiplier = -1.0;
 };
 
 }  // namespace frc

wpilibc/src/main/native/include/Drive/MecanumDrive.h

@@ -76,6 +76,9 @@ class MecanumDrive : public RobotDriveBase {
                       double gyroAngle = 0.0);
   void DrivePolar(double magnitude, double angle, double zRotation);
 
+  bool IsRightSideInverted() const;
+  void SetRightSideInverted(bool rightSideInverted);
+
   void StopMotor() override;
   void GetDescription(wpi::raw_ostream& desc) const override;
 
@@ -87,6 +90,8 @@ class MecanumDrive : public RobotDriveBase {
   SpeedController& m_frontRightMotor;
   SpeedController& m_rearRightMotor;
 
+  double m_rightSideInvertMultiplier = -1.0;
+
   bool reported = false;
 };
 

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

@@ -103,6 +103,7 @@ public class DifferentialDrive extends RobotDriveBase {
   private double m_quickStopThreshold = kDefaultQuickStopThreshold;
   private double m_quickStopAlpha = kDefaultQuickStopAlpha;
   private double m_quickStopAccumulator = 0.0;
+  private double m_rightSideInvertMultiplier = -1.0;
   private boolean m_reported = false;
 
   /**
@@ -187,7 +188,7 @@ public class DifferentialDrive extends RobotDriveBase {
     }
 
     m_leftMotor.set(limit(leftMotorOutput) * m_maxOutput);
-    m_rightMotor.set(-limit(rightMotorOutput) * m_maxOutput);
+    m_rightMotor.set(limit(rightMotorOutput) * m_maxOutput * m_rightSideInvertMultiplier);
 
     m_safetyHelper.feed();
   }
@@ -270,7 +271,7 @@ public class DifferentialDrive extends RobotDriveBase {
     }
 
     m_leftMotor.set(leftMotorOutput * m_maxOutput);
-    m_rightMotor.set(-rightMotorOutput * m_maxOutput);
+    m_rightMotor.set(rightMotorOutput * m_maxOutput * m_rightSideInvertMultiplier);
 
     m_safetyHelper.feed();
   }
@@ -317,7 +318,7 @@ public class DifferentialDrive extends RobotDriveBase {
     }
 
     m_leftMotor.set(leftSpeed * m_maxOutput);
-    m_rightMotor.set(-rightSpeed * m_maxOutput);
+    m_rightMotor.set(rightSpeed * m_maxOutput * m_rightSideInvertMultiplier);
 
     m_safetyHelper.feed();
   }
@@ -353,6 +354,24 @@ public class DifferentialDrive extends RobotDriveBase {
     m_quickStopAlpha = alpha;
   }
 
+  /**
+   * Gets if the power sent to the right side of the drivetrain is multipled 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 multipled by -1.
+   *
+   * @param rightSideInverted true if right side power should be multipled by -1
+   */
+  public void setRightSideInverted(boolean rightSideInverted) {
+    m_rightSideInvertMultiplier = rightSideInverted ? -1.0 : 1.0;
+  }
+
   @Override
   public void stopMotor() {
     m_leftMotor.stopMotor();
@@ -371,7 +390,7 @@ public class DifferentialDrive extends RobotDriveBase {
     builder.addDoubleProperty("Left Motor Speed", m_leftMotor::get, m_leftMotor::set);
     builder.addDoubleProperty(
         "Right Motor Speed",
-        () -> -m_rightMotor.get(),
-        x -> m_rightMotor.set(-x));
+        () -> m_rightMotor.get() * m_rightSideInvertMultiplier,
+        x -> m_rightMotor.set(x * m_rightSideInvertMultiplier));
   }
 }

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

@@ -64,6 +64,7 @@ public class MecanumDrive extends RobotDriveBase {
   private SpeedController m_frontRightMotor;
   private SpeedController m_rearRightMotor;
 
+  private double m_rightSideInvertMultiplier = -1.0;
   private boolean m_reported = false;
 
   /**
@@ -134,16 +135,18 @@ public class MecanumDrive extends RobotDriveBase {
 
     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.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;
+    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_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_rearRightMotor.set(wheelSpeeds[MotorType.kRearRight.value] * m_maxOutput
+        * m_rightSideInvertMultiplier);
 
     m_safetyHelper.feed();
   }
@@ -170,6 +173,24 @@ public class MecanumDrive extends RobotDriveBase {
                    magnitude * Math.cos(angle * (Math.PI / 180.0)), zRotation, 0.0);
   }
 
+  /**
+   * Gets if the power sent to the right side of the drivetrain is multipled 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 multipled by -1.
+   *
+   * @param rightSideInverted true if right side power should be multipled by -1
+   */
+  public void setRightSideInverted(boolean rightSideInverted) {
+    m_rightSideInvertMultiplier = rightSideInverted ? -1.0 : 1.0;
+  }
+
   @Override
   public void stopMotor() {
     m_frontLeftMotor.stopMotor();
@@ -187,13 +208,17 @@ public class MecanumDrive extends RobotDriveBase {
   @Override
   public void initSendable(SendableBuilder builder) {
     builder.setSmartDashboardType("MecanumDrive");
-    builder.addDoubleProperty("Front Left Motor Speed", m_frontLeftMotor::get,
+    builder.addDoubleProperty("Front Left Motor Speed",
+        m_frontLeftMotor::get,
         m_frontLeftMotor::set);
-    builder.addDoubleProperty("Front Right Motor Speed", () -> -m_frontRightMotor.get(),
-        value -> m_frontRightMotor.set(-value));
-    builder.addDoubleProperty("Rear Left Motor Speed", m_rearLeftMotor::get,
+    builder.addDoubleProperty("Front Right Motor Speed",
+        () -> m_frontRightMotor.get() * m_rightSideInvertMultiplier,
+        value -> m_frontRightMotor.set(value * m_rightSideInvertMultiplier));
+    builder.addDoubleProperty("Rear Left Motor Speed",
+        m_rearLeftMotor::get,
         m_rearLeftMotor::set);
-    builder.addDoubleProperty("Rear Right Motor Speed", () -> -m_rearRightMotor.get(),
-        value -> m_rearRightMotor.set(-value));
+    builder.addDoubleProperty("Rear Right Motor Speed",
+        () -> m_rearRightMotor.get() * m_rightSideInvertMultiplier,
+        value -> m_rearRightMotor.set(value * m_rightSideInvertMultiplier));
   }
 }