diff --git a/wpilibc/src/main/native/cpp/Drive/DifferentialDrive.cpp b/wpilibc/src/main/native/cpp/Drive/DifferentialDrive.cpp index ceb72d1b31..6143565435 100644 --- a/wpilibc/src/main/native/cpp/Drive/DifferentialDrive.cpp +++ b/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); + }); } diff --git a/wpilibc/src/main/native/cpp/Drive/MecanumDrive.cpp b/wpilibc/src/main/native/cpp/Drive/MecanumDrive.cpp index 11310c449c..c378a72115 100644 --- a/wpilibc/src/main/native/cpp/Drive/MecanumDrive.cpp +++ b/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); + }); } diff --git a/wpilibc/src/main/native/include/Drive/DifferentialDrive.h b/wpilibc/src/main/native/include/Drive/DifferentialDrive.h index 4f249588f4..70a345fbc1 100644 --- a/wpilibc/src/main/native/include/Drive/DifferentialDrive.h +++ b/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 diff --git a/wpilibc/src/main/native/include/Drive/MecanumDrive.h b/wpilibc/src/main/native/include/Drive/MecanumDrive.h index 047bf73595..11d81355de 100644 --- a/wpilibc/src/main/native/include/Drive/MecanumDrive.h +++ b/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; }; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java index 1ab31156a6..08bf753282 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java +++ b/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)); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java index c2eaa23462..958f37a74c 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java +++ b/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)); } }