diff --git a/wpilibcExamples/src/main/cpp/examples/ArcadeDrive/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ArcadeDrive/cpp/Robot.cpp index abe79f48a8..f2198552c4 100644 --- a/wpilibcExamples/src/main/cpp/examples/ArcadeDrive/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/ArcadeDrive/cpp/Robot.cpp @@ -18,6 +18,13 @@ class Robot : public frc::TimedRobot { frc::Joystick m_stick{0}; public: + void RobotInit() override { + // We need to invert one side of the drivetrain so that positive voltages + // result in both sides moving forward. Depending on how your robot's + // gearbox is constructed, you might have to invert the left side instead. + m_rightMotor.SetInverted(true); + } + void TeleopPeriodic() override { // Drive with arcade style m_robotDrive.ArcadeDrive(m_stick.GetY(), m_stick.GetX()); diff --git a/wpilibcExamples/src/main/cpp/examples/ArcadeDriveXboxController/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ArcadeDriveXboxController/cpp/Robot.cpp index 644e35fecf..23c9a569dd 100644 --- a/wpilibcExamples/src/main/cpp/examples/ArcadeDriveXboxController/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/ArcadeDriveXboxController/cpp/Robot.cpp @@ -18,12 +18,19 @@ class Robot : public frc::TimedRobot { frc::XboxController m_driverController{0}; public: + void RobotInit() override { + // We need to invert one side of the drivetrain so that positive voltages + // result in both sides moving forward. Depending on how your robot's + // gearbox is constructed, you might have to invert the left side instead. + m_rightMotor.SetInverted(true); + } + void TeleopPeriodic() override { // Drive with split arcade style // That means that the Y axis of the left stick moves forward // and backward, and the X of the right stick turns left and right. - m_robotDrive.ArcadeDrive(m_driverController.GetLeftY(), - m_driverController.GetRightX()); + m_robotDrive.ArcadeDrive(-m_driverController.GetLeftY(), + -m_driverController.GetRightX()); } }; diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/RobotContainer.cpp index 48310da26f..1dfff1253a 100644 --- a/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/RobotContainer.cpp +++ b/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/RobotContainer.cpp @@ -17,8 +17,8 @@ RobotContainer::RobotContainer() { // Set up default drive command m_drive.SetDefaultCommand(frc2::RunCommand( [this] { - m_drive.ArcadeDrive(m_driverController.GetLeftY(), - m_driverController.GetRightX()); + m_drive.ArcadeDrive(-m_driverController.GetLeftY(), + -m_driverController.GetRightX()); }, {&m_drive})); } diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/subsystems/DriveSubsystem.cpp index 5ba38b8736..aeeac3ddf9 100644 --- a/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/subsystems/DriveSubsystem.cpp +++ b/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/subsystems/DriveSubsystem.cpp @@ -16,6 +16,10 @@ DriveSubsystem::DriveSubsystem() // Set the distance per pulse for the encoders m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse); m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse); + // We need to invert one side of the drivetrain so that positive voltages + // result in both sides moving forward. Depending on how your robot's + // gearbox is constructed, you might have to invert the left side instead. + m_rightMotors.SetInverted(true); } void DriveSubsystem::Periodic() { diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/RobotContainer.cpp index b5a4420fb0..bf62e3c8df 100644 --- a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/RobotContainer.cpp +++ b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/RobotContainer.cpp @@ -17,8 +17,8 @@ RobotContainer::RobotContainer() { // Set up default drive command m_drive.SetDefaultCommand(frc2::RunCommand( [this] { - m_drive.ArcadeDrive(m_driverController.GetLeftY(), - m_driverController.GetRightX()); + m_drive.ArcadeDrive(-m_driverController.GetLeftY(), + -m_driverController.GetRightX()); }, {&m_drive})); } diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/DriveSubsystem.cpp index 03d341598d..fac0df9493 100644 --- a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/DriveSubsystem.cpp +++ b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/DriveSubsystem.cpp @@ -16,6 +16,10 @@ DriveSubsystem::DriveSubsystem() // Set the distance per pulse for the encoders m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse); m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse); + // We need to invert one side of the drivetrain so that positive voltages + // result in both sides moving forward. Depending on how your robot's + // gearbox is constructed, you might have to invert the left side instead. + m_rightMotors.SetInverted(true); } void DriveSubsystem::Periodic() { diff --git a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/RobotContainer.cpp index fb879fe9b8..31293409a5 100644 --- a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/RobotContainer.cpp +++ b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/RobotContainer.cpp @@ -18,8 +18,8 @@ RobotContainer::RobotContainer() { // Set up default drive command m_drive.SetDefaultCommand(frc2::RunCommand( [this] { - m_drive.ArcadeDrive(m_driverController.GetLeftY(), - m_driverController.GetRightX()); + m_drive.ArcadeDrive(-m_driverController.GetLeftY(), + -m_driverController.GetRightX()); }, {&m_drive})); } diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/RobotContainer.cpp index b781422cba..a8effa33d7 100644 --- a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/RobotContainer.cpp +++ b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/RobotContainer.cpp @@ -16,8 +16,8 @@ RobotContainer::RobotContainer() { // Set up default drive command m_drive.SetDefaultCommand(frc2::RunCommand( [this] { - m_drive.ArcadeDrive(m_driverController.GetLeftY(), - m_driverController.GetRightX()); + m_drive.ArcadeDrive(-m_driverController.GetLeftY(), + -m_driverController.GetRightX()); }, {&m_drive})); } diff --git a/wpilibcExamples/src/main/cpp/examples/GettingStarted/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/GettingStarted/cpp/Robot.cpp index 6b7d2b6892..42f7bdc07b 100644 --- a/wpilibcExamples/src/main/cpp/examples/GettingStarted/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/GettingStarted/cpp/Robot.cpp @@ -11,7 +11,11 @@ class Robot : public frc::TimedRobot { public: Robot() { + m_right.SetInverted(true); m_robotDrive.SetExpiration(100_ms); + // We need to invert one side of the drivetrain so that positive voltages + // result in both sides moving forward. Depending on how your robot's + // gearbox is constructed, you might have to invert the left side instead. m_timer.Start(); } diff --git a/wpilibcExamples/src/main/cpp/examples/Gyro/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/Gyro/cpp/Robot.cpp index 1dda356ff5..744dd590b2 100644 --- a/wpilibcExamples/src/main/cpp/examples/Gyro/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/Gyro/cpp/Robot.cpp @@ -17,7 +17,13 @@ */ class Robot : public frc::TimedRobot { public: - void RobotInit() override { m_gyro.SetSensitivity(kVoltsPerDegreePerSecond); } + void RobotInit() override { + m_gyro.SetSensitivity(kVoltsPerDegreePerSecond); + // We need to invert one side of the drivetrain so that positive voltages + // result in both sides moving forward. Depending on how your robot's + // gearbox is constructed, you might have to invert the left side instead. + m_right.SetInverted(true); + } /** * The motor speed is set from the joystick while the DifferentialDrive diff --git a/wpilibcExamples/src/main/cpp/examples/GyroMecanum/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/GyroMecanum/cpp/Robot.cpp index 58858733f4..a39967af00 100644 --- a/wpilibcExamples/src/main/cpp/examples/GyroMecanum/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/GyroMecanum/cpp/Robot.cpp @@ -16,10 +16,10 @@ class Robot : public frc::TimedRobot { public: void RobotInit() override { - // Invert the left side motors. You may need to change or remove this to + // Invert the right side motors. You may need to change or remove this to // match your robot. - m_frontLeft.SetInverted(true); - m_rearLeft.SetInverted(true); + m_frontRight.SetInverted(true); + m_rearRight.SetInverted(true); m_gyro.SetSensitivity(kVoltsPerDegreePerSecond); } diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/RobotContainer.cpp index b46efd4226..9d3aab2488 100644 --- a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/RobotContainer.cpp +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/RobotContainer.cpp @@ -27,8 +27,8 @@ RobotContainer::RobotContainer() { // Set up default drive command m_drive.SetDefaultCommand(DefaultDrive( - &m_drive, [this] { return m_driverController.GetLeftY(); }, - [this] { return m_driverController.GetRightX(); })); + &m_drive, [this] { return -m_driverController.GetLeftY(); }, + [this] { return -m_driverController.GetRightX(); })); } void RobotContainer::ConfigureButtonBindings() { diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h index 0aa30868ba..9ec5fc5b77 100644 --- a/wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h +++ b/wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h @@ -20,7 +20,14 @@ */ class Drivetrain { public: - Drivetrain() { m_gyro.Reset(); } + Drivetrain() { + m_gyro.Reset(); + // We need to invert one side of the drivetrain so that positive voltages + // result in both sides moving forward. Depending on how your robot's + // gearbox is constructed, you might have to invert the left side instead. + m_frontRightMotor.SetInverted(true); + m_backRightMotor.SetInverted(true); + } frc::MecanumDriveWheelSpeeds GetCurrentState() const; void SetSpeeds(const frc::MecanumDriveWheelSpeeds& wheelSpeeds); diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/subsystems/DriveSubsystem.cpp index 4cc4e47a1c..9e06068512 100644 --- a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/subsystems/DriveSubsystem.cpp +++ b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/subsystems/DriveSubsystem.cpp @@ -34,6 +34,11 @@ DriveSubsystem::DriveSubsystem() m_rearLeftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse); m_frontRightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse); m_rearRightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse); + // We need to invert one side of the drivetrain so that positive voltages + // result in both sides moving forward. Depending on how your robot's + // gearbox is constructed, you might have to invert the left side instead. + m_frontRight.SetInverted(true); + m_rearRight.SetInverted(true); } void DriveSubsystem::Periodic() { diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumDrive/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/MecanumDrive/cpp/Robot.cpp index 822e9ee675..cc9632e009 100644 --- a/wpilibcExamples/src/main/cpp/examples/MecanumDrive/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/MecanumDrive/cpp/Robot.cpp @@ -14,10 +14,10 @@ class Robot : public frc::TimedRobot { public: void RobotInit() override { - // Invert the left side motors. You may need to change or remove this to + // Invert the right side motors. You may need to change or remove this to // match your robot. - m_frontLeft.SetInverted(true); - m_rearLeft.SetInverted(true); + m_frontRight.SetInverted(true); + m_rearRight.SetInverted(true); } void TeleopPeriodic() override { diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/include/Drivetrain.h index d084fa579f..3eea4a2557 100644 --- a/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/include/Drivetrain.h +++ b/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/include/Drivetrain.h @@ -21,7 +21,14 @@ */ class Drivetrain { public: - Drivetrain() { m_gyro.Reset(); } + Drivetrain() { + m_gyro.Reset(); + // We need to invert one side of the drivetrain so that positive voltages + // result in both sides moving forward. Depending on how your robot's + // gearbox is constructed, you might have to invert the left side instead. + m_frontRightMotor.SetInverted(true); + m_backRightMotor.SetInverted(true); + } frc::MecanumDriveWheelSpeeds GetCurrentState() const; void SetSpeeds(const frc::MecanumDriveWheelSpeeds& wheelSpeeds); diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp index 78355a2d21..a5b853ea33 100644 --- a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp +++ b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp @@ -42,7 +42,7 @@ void DriveSubsystem::ArcadeDrive(double fwd, double rot) { void DriveSubsystem::TankDriveVolts(units::volt_t left, units::volt_t right) { m_leftMotors.SetVoltage(left); - m_rightMotors.SetVoltage(-right); + m_rightMotors.SetVoltage(right); m_drive.Feed(); } diff --git a/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/cpp/Drivetrain.cpp index 05d99f37f7..20b8bca59e 100644 --- a/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/cpp/Drivetrain.cpp +++ b/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/cpp/Drivetrain.cpp @@ -43,7 +43,7 @@ void Drivetrain::SimulationPeriodic() { // voltages make the right side move forward. m_drivetrainSimulator.SetInputs(units::volt_t{m_leftLeader.Get()} * frc::RobotController::GetInputVoltage(), - units::volt_t{-m_rightLeader.Get()} * + units::volt_t{m_rightLeader.Get()} * frc::RobotController::GetInputVoltage()); m_drivetrainSimulator.Update(20_ms); diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/RobotContainer.cpp index 554de50aa1..82f470061e 100644 --- a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/RobotContainer.cpp +++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/RobotContainer.cpp @@ -28,8 +28,8 @@ RobotContainer::RobotContainer() { // Set up default drive command m_drive.SetDefaultCommand(frc2::RunCommand( [this] { - m_drive.ArcadeDrive(m_driverController.GetLeftY(), - m_driverController.GetRightX()); + m_drive.ArcadeDrive(-m_driverController.GetLeftY(), + -m_driverController.GetRightX()); }, {&m_drive})); } diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/subsystems/DriveSubsystem.cpp index a94b860d71..0971549b76 100644 --- a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/subsystems/DriveSubsystem.cpp +++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/subsystems/DriveSubsystem.cpp @@ -42,7 +42,7 @@ void DriveSubsystem::SimulationPeriodic() { // voltages make the right side move forward. m_drivetrainSimulator.SetInputs(units::volt_t{m_leftMotors.Get()} * frc::RobotController::GetInputVoltage(), - units::volt_t{-m_rightMotors.Get()} * + units::volt_t{m_rightMotors.Get()} * frc::RobotController::GetInputVoltage()); m_drivetrainSimulator.Update(20_ms); @@ -64,7 +64,7 @@ void DriveSubsystem::ArcadeDrive(double fwd, double rot) { void DriveSubsystem::TankDriveVolts(units::volt_t left, units::volt_t right) { m_leftMotors.SetVoltage(left); - m_rightMotors.SetVoltage(-right); + m_rightMotors.SetVoltage(right); m_drive.Feed(); } diff --git a/wpilibcExamples/src/main/cpp/examples/TankDriveXboxController/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/TankDriveXboxController/cpp/Robot.cpp index 466fdcd4ff..b8cff3271e 100644 --- a/wpilibcExamples/src/main/cpp/examples/TankDriveXboxController/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/TankDriveXboxController/cpp/Robot.cpp @@ -27,8 +27,8 @@ class Robot : public frc::TimedRobot { void TeleopPeriodic() override { // Drive with tank style - m_robotDrive.TankDrive(m_driverController.GetLeftY(), - m_driverController.GetRightY()); + m_robotDrive.TankDrive(-m_driverController.GetLeftY(), + -m_driverController.GetRightY()); } }; diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrive/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrive/Robot.java index e35243de58..d15c9724b4 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrive/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrive/Robot.java @@ -19,6 +19,14 @@ public class Robot extends TimedRobot { private final DifferentialDrive m_robotDrive = new DifferentialDrive(m_leftMotor, m_rightMotor); private final Joystick m_stick = new Joystick(0); + @Override + public void robotInit() { + // We need to invert one side of the drivetrain so that positive voltages + // result in both sides moving forward. Depending on how your robot's + // gearbox is constructed, you might have to invert the left side instead. + m_rightMotor.setInverted(true); + } + @Override public void teleopPeriodic() { // Drive with arcade drive. diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrivexboxcontroller/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrivexboxcontroller/Robot.java index 0acc8dcd4e..aa0f9a20ea 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrivexboxcontroller/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrivexboxcontroller/Robot.java @@ -19,11 +19,19 @@ public class Robot extends TimedRobot { private final DifferentialDrive m_robotDrive = new DifferentialDrive(m_leftMotor, m_rightMotor); private final XboxController m_driverController = new XboxController(0); + @Override + public void robotInit() { + // We need to invert one side of the drivetrain so that positive voltages + // result in both sides moving forward. Depending on how your robot's + // gearbox is constructed, you might have to invert the left side instead. + m_rightMotor.setInverted(true); + } + @Override public void teleopPeriodic() { // Drive with split arcade drive. // That means that the Y axis of the left stick moves forward // and backward, and the X of the right stick turns left and right. - m_robotDrive.arcadeDrive(m_driverController.getLeftY(), m_driverController.getRightX()); + m_robotDrive.arcadeDrive(-m_driverController.getLeftY(), -m_driverController.getRightX()); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/RobotContainer.java index b0d019e8fb..e2240b20ed 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/RobotContainer.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/RobotContainer.java @@ -42,7 +42,7 @@ public class RobotContainer { new RunCommand( () -> m_robotDrive.arcadeDrive( - m_driverController.getLeftY(), m_driverController.getRightX()), + -m_driverController.getLeftY(), -m_driverController.getRightX()), m_robotDrive)); } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/DriveSubsystem.java index 9338f309f7..4ecaa77231 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/DriveSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/DriveSubsystem.java @@ -46,6 +46,11 @@ public class DriveSubsystem extends SubsystemBase { // Sets the distance per pulse for the encoders m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse); m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse); + + // We need to invert one side of the drivetrain so that positive voltages + // result in both sides moving forward. Depending on how your robot's + // gearbox is constructed, you might have to invert the left side instead. + m_rightMotors.setInverted(true); } /** diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/RobotContainer.java index b0c3f76416..f7f3768238 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/RobotContainer.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/RobotContainer.java @@ -42,7 +42,7 @@ public class RobotContainer { new RunCommand( () -> m_robotDrive.arcadeDrive( - m_driverController.getLeftY(), m_driverController.getRightX()), + -m_driverController.getLeftY(), -m_driverController.getRightX()), m_robotDrive)); } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/DriveSubsystem.java index 8017719044..939bcc3237 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/DriveSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/DriveSubsystem.java @@ -46,6 +46,11 @@ public class DriveSubsystem extends SubsystemBase { // Sets the distance per pulse for the encoders m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse); m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse); + + // We need to invert one side of the drivetrain so that positive voltages + // result in both sides moving forward. Depending on how your robot's + // gearbox is constructed, you might have to invert the left side instead. + m_rightMotors.setInverted(true); } /** diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/RobotContainer.java index 78b7f81803..9fab6ddfef 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/RobotContainer.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/RobotContainer.java @@ -44,7 +44,7 @@ public class RobotContainer { new RunCommand( () -> m_robotDrive.arcadeDrive( - m_driverController.getLeftY(), m_driverController.getRightX()), + -m_driverController.getLeftY(), -m_driverController.getRightX()), m_robotDrive)); } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/RobotContainer.java index 5d228d0cd3..679771ee79 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/RobotContainer.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/RobotContainer.java @@ -67,7 +67,7 @@ public class RobotContainer { new RunCommand( () -> m_robotDrive.arcadeDrive( - m_driverController.getLeftY(), m_driverController.getRightX()), + -m_driverController.getLeftY(), -m_driverController.getRightX()), m_robotDrive)); } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gettingstarted/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gettingstarted/Robot.java index 0a9a2b034d..f9499f5191 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gettingstarted/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gettingstarted/Robot.java @@ -17,8 +17,9 @@ import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax; * directory. */ public class Robot extends TimedRobot { - private final DifferentialDrive m_robotDrive = - new DifferentialDrive(new PWMSparkMax(0), new PWMSparkMax(1)); + private final PWMSparkMax m_leftDrive = new PWMSparkMax(0); + private final PWMSparkMax m_rightDrive = new PWMSparkMax(1); + private final DifferentialDrive m_robotDrive = new DifferentialDrive(m_leftDrive, m_rightDrive); private final Joystick m_stick = new Joystick(0); private final Timer m_timer = new Timer(); @@ -27,7 +28,12 @@ public class Robot extends TimedRobot { * initialization code. */ @Override - public void robotInit() {} + public void robotInit() { + // We need to invert one side of the drivetrain so that positive voltages + // result in both sides moving forward. Depending on how your robot's + // gearbox is constructed, you might have to invert the left side instead. + m_rightDrive.setInverted(true); + } /** This function is run once each time the robot enters autonomous mode. */ @Override diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyro/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyro/Robot.java index 3d8353bd90..a8bbb239c9 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyro/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyro/Robot.java @@ -28,14 +28,19 @@ public class Robot extends TimedRobot { private static final int kGyroPort = 0; private static final int kJoystickPort = 0; - private final DifferentialDrive m_myRobot = - new DifferentialDrive(new PWMSparkMax(kLeftMotorPort), new PWMSparkMax(kRightMotorPort)); + private final PWMSparkMax m_leftDrive = new PWMSparkMax(kLeftMotorPort); + private final PWMSparkMax m_rightDrive = new PWMSparkMax(kRightMotorPort); + private final DifferentialDrive m_myRobot = new DifferentialDrive(m_leftDrive, m_rightDrive); private final AnalogGyro m_gyro = new AnalogGyro(kGyroPort); private final Joystick m_joystick = new Joystick(kJoystickPort); @Override public void robotInit() { m_gyro.setSensitivity(kVoltsPerDegreePerSecond); + // We need to invert one side of the drivetrain so that positive voltages + // result in both sides moving forward. Depending on how your robot's + // gearbox is constructed, you might have to invert the left side instead. + m_rightDrive.setInverted(true); } /** diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyromecanum/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyromecanum/Robot.java index f4b9a97e8c..bf26aa9015 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyromecanum/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyromecanum/Robot.java @@ -37,10 +37,10 @@ public class Robot extends TimedRobot { PWMSparkMax frontRight = new PWMSparkMax(kFrontRightChannel); PWMSparkMax rearRight = new PWMSparkMax(kRearRightChannel); - // Invert the left side motors. + // Invert the right side motors. // You may need to change or remove this to match your robot. - frontLeft.setInverted(true); - rearLeft.setInverted(true); + frontRight.setInverted(true); + rearRight.setInverted(true); m_robotDrive = new MecanumDrive(frontLeft, rearLeft, frontRight, rearRight); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java index c660ae2fa0..7ed24a79b3 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java @@ -56,6 +56,11 @@ public class Drivetrain { /** Constructs a MecanumDrive and resets the gyro. */ public Drivetrain() { m_gyro.reset(); + // We need to invert one side of the drivetrain so that positive voltages + // result in both sides moving forward. Depending on how your robot's + // gearbox is constructed, you might have to invert the left side instead. + m_frontRightMotor.setInverted(true); + m_backRightMotor.setInverted(true); } /** diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java index 23da86770e..7874d82c53 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java @@ -67,6 +67,11 @@ public class DriveSubsystem extends SubsystemBase { m_rearLeftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse); m_frontRightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse); m_rearRightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse); + // We need to invert one side of the drivetrain so that positive voltages + // result in both sides moving forward. Depending on how your robot's + // gearbox is constructed, you might have to invert the left side instead. + m_frontRight.setInverted(true); + m_rearRight.setInverted(true); } @Override diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdrive/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdrive/Robot.java index 80c16b52fb..9720f48ad3 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdrive/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdrive/Robot.java @@ -28,10 +28,10 @@ public class Robot extends TimedRobot { PWMSparkMax frontRight = new PWMSparkMax(kFrontRightChannel); PWMSparkMax rearRight = new PWMSparkMax(kRearRightChannel); - // Invert the left side motors. + // Invert the right side motors. // You may need to change or remove this to match your robot. - frontLeft.setInverted(true); - rearLeft.setInverted(true); + frontRight.setInverted(true); + rearRight.setInverted(true); m_robotDrive = new MecanumDrive(frontLeft, rearLeft, frontRight, rearRight); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java index ba544b2de5..fa2813f784 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java @@ -68,6 +68,11 @@ public class Drivetrain { /** Constructs a MecanumDrive and resets the gyro. */ public Drivetrain() { m_gyro.reset(); + // We need to invert one side of the drivetrain so that positive voltages + // result in both sides moving forward. Depending on how your robot's + // gearbox is constructed, you might have to invert the left side instead. + m_frontRightMotor.setInverted(true); + m_backRightMotor.setInverted(true); } /** diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java index 680af28962..a7515c2370 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java @@ -53,7 +53,7 @@ public class RobotContainer { new RunCommand( () -> m_robotDrive.arcadeDrive( - m_driverController.getLeftY(), m_driverController.getRightX()), + -m_driverController.getLeftY(), -m_driverController.getRightX()), m_robotDrive)); } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java index 8ca929d04b..b2fe2f70c3 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java @@ -120,7 +120,7 @@ public class DriveSubsystem extends SubsystemBase { */ public void tankDriveVolts(double leftVolts, double rightVolts) { m_leftMotors.setVoltage(leftVolts); - m_rightMotors.setVoltage(-rightVolts); + m_rightMotors.setVoltage(rightVolts); m_drive.feed(); } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java index ea80fd2ce2..db8ab556da 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java @@ -145,7 +145,7 @@ public class Drivetrain { // voltages make the right side move forward. m_drivetrainSimulator.setInputs( m_leftLeader.get() * RobotController.getInputVoltage(), - -m_rightLeader.get() * RobotController.getInputVoltage()); + m_rightLeader.get() * RobotController.getInputVoltage()); m_drivetrainSimulator.update(0.02); m_leftEncoderSim.setDistance(m_drivetrainSimulator.getLeftPositionMeters()); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/RobotContainer.java index 4636c37628..c1cc6c0a82 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/RobotContainer.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/RobotContainer.java @@ -50,7 +50,7 @@ public class RobotContainer { new RunCommand( () -> m_robotDrive.arcadeDrive( - -m_driverController.getLeftY(), m_driverController.getRightX()), + -m_driverController.getLeftY(), -m_driverController.getRightX()), m_robotDrive)); } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/subsystems/DriveSubsystem.java index 98037c33c5..e9742696d4 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/subsystems/DriveSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/subsystems/DriveSubsystem.java @@ -122,7 +122,7 @@ public class DriveSubsystem extends SubsystemBase { // move forward. m_drivetrainSimulator.setInputs( m_leftMotors.get() * RobotController.getBatteryVoltage(), - -m_rightMotors.get() * RobotController.getBatteryVoltage()); + m_rightMotors.get() * RobotController.getBatteryVoltage()); m_drivetrainSimulator.update(0.020); m_leftEncoderSim.setDistance(m_drivetrainSimulator.getLeftPositionMeters()); @@ -194,7 +194,7 @@ public class DriveSubsystem extends SubsystemBase { rightVolts *= batteryVoltage / 12.0; } m_leftMotors.setVoltage(leftVolts); - m_rightMotors.setVoltage(-rightVolts); + m_rightMotors.setVoltage(rightVolts); m_drive.feed(); } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrivexboxcontroller/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrivexboxcontroller/Robot.java index f05e22c5ea..c6a985af5c 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrivexboxcontroller/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrivexboxcontroller/Robot.java @@ -33,6 +33,6 @@ public class Robot extends TimedRobot { // That means that the Y axis of the left stick moves the left side // of the robot forward and backward, and the Y axis of the right stick // moves the right side of the robot forward and backward. - m_robotDrive.tankDrive(m_driverController.getLeftY(), m_driverController.getRightY()); + m_robotDrive.tankDrive(-m_driverController.getLeftY(), -m_driverController.getRightY()); } }