diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/Robot.java index f04b6a156a..baecb65bf2 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/Robot.java @@ -16,7 +16,6 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler; */ public class Robot extends TimedRobot { private Command m_autonomousCommand; - private RobotContainer m_robotContainer; /** @@ -56,6 +55,8 @@ public class Robot extends TimedRobot { /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ @Override public void autonomousInit() { + + // Get selected routine from the SmartDashboard m_autonomousCommand = m_robotContainer.getAutonomousCommand(); // schedule the autonomous command (example) @@ -70,9 +71,9 @@ public class Robot extends TimedRobot { @Override public void teleopInit() { - // This makes sure that the autonomous stops running when - // teleop starts running. If you want the autonomous to - // continue until interrupted by another command, remove + // This makes sure that the autonomous stops running which will + // use the default command which is ArcadeDrive. If you want the autonomous + // to continue until interrupted by another command, remove // this line or comment it out. if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/RobotContainer.java index 5e3a7facc4..aec0415ef8 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/RobotContainer.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/RobotContainer.java @@ -7,10 +7,14 @@ package edu.wpi.first.wpilibj.examples.romireference; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj.examples.romireference.commands.TeleopArcadeDrive; +import edu.wpi.first.wpilibj.examples.romireference.commands.ArcadeDrive; +import edu.wpi.first.wpilibj.examples.romireference.commands.AutonomousDistance; +import edu.wpi.first.wpilibj.examples.romireference.commands.AutonomousTime; import edu.wpi.first.wpilibj.examples.romireference.subsystems.Drivetrain; import edu.wpi.first.wpilibj.examples.romireference.subsystems.OnBoardIO; import edu.wpi.first.wpilibj.examples.romireference.subsystems.OnBoardIO.ChannelMode; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.button.Button; @@ -29,6 +33,9 @@ public class RobotContainer { // Assumes a gamepad plugged into channnel 0 private final Joystick m_controller = new Joystick(0); + // Create SmartDashboard chooser for autonomous routines + private final SendableChooser m_chooser = new SendableChooser<>(); + // NOTE: The I/O pin functionality of the 5 exposed I/O pins depends on the hardware "overlay" // that is specified when launching the wpilib-ws server on the Romi raspberry pi. // By default, the following are available (listed in order from inside of the board to outside): @@ -53,16 +60,20 @@ public class RobotContainer { * edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureButtonBindings() { - // Also set default commands here - m_drivetrain.setDefaultCommand( - new TeleopArcadeDrive( - m_drivetrain, () -> m_controller.getRawAxis(1), () -> -m_controller.getRawAxis(2))); + // Default command is arcade drive. This will run unless another command + // is scheduled over it. + m_drivetrain.setDefaultCommand(getArcadeDriveCommand()); // Example of how to use the onboard IO Button onboardButtonA = new Button(m_onboardIO::getButtonAPressed); onboardButtonA .whenActive(new PrintCommand("Button A Pressed")) .whenInactive(new PrintCommand("Button A Released")); + + // Setup SmartDashboard options + m_chooser.setDefaultOption("Auto Routine Distance", new AutonomousDistance(m_drivetrain)); + m_chooser.addOption("Auto Routine Time", new AutonomousTime(m_drivetrain)); + SmartDashboard.putData(m_chooser); } /** @@ -71,7 +82,16 @@ public class RobotContainer { * @return the command to run in autonomous */ public Command getAutonomousCommand() { - // An ExampleCommand will run in autonomous - return null; + return m_chooser.getSelected(); + } + + /** + * Use this to pass the teleop command to the main {@link Robot} class. + * + * @return the command to run in teleop + */ + public Command getArcadeDriveCommand() { + return new ArcadeDrive( + m_drivetrain, () -> m_controller.getRawAxis(1), () -> -m_controller.getRawAxis(2)); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/TeleopArcadeDrive.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/ArcadeDrive.java similarity index 75% rename from wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/TeleopArcadeDrive.java rename to wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/ArcadeDrive.java index 1b171cca0c..11725a894b 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/TeleopArcadeDrive.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/ArcadeDrive.java @@ -8,20 +8,26 @@ import edu.wpi.first.wpilibj.examples.romireference.subsystems.Drivetrain; import edu.wpi.first.wpilibj2.command.CommandBase; import java.util.function.Supplier; -public class TeleopArcadeDrive extends CommandBase { +public class ArcadeDrive extends CommandBase { private final Drivetrain m_drivetrain; private final Supplier m_xaxisSpeedSupplier; private final Supplier m_zaxisRotateSupplier; - /** Creates a new TeleopArcadeDrive. */ - public TeleopArcadeDrive( + /** + * Creates a new ArcadeDrive. This command will drive your robot according to the speed supplier + * lambdas. This command does not terminate. + * + * @param drivetrain The drivetrain subsystem on which this command will run + * @param xaxisSpeedSupplier Lambda supplier of forward/backward speed + * @param zaxisRotateSuppplier Lambda supplier of rotational speed + */ + public ArcadeDrive( Drivetrain drivetrain, Supplier xaxisSpeedSupplier, Supplier zaxisRotateSuppplier) { m_drivetrain = drivetrain; m_xaxisSpeedSupplier = xaxisSpeedSupplier; m_zaxisRotateSupplier = zaxisRotateSuppplier; - addRequirements(drivetrain); } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/AutonomousDistance.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/AutonomousDistance.java new file mode 100644 index 0000000000..d48b2b4637 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/AutonomousDistance.java @@ -0,0 +1,24 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.examples.romireference.commands; + +import edu.wpi.first.wpilibj.examples.romireference.subsystems.Drivetrain; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +public class AutonomousDistance extends SequentialCommandGroup { + /** + * Creates a new Autonomous Drive based on distance. This will drive out for a specified distance, + * turn around and drive back. + * + * @param drivetrain The drivetrain subsystem on which this command will run + */ + public AutonomousDistance(Drivetrain drivetrain) { + addCommands( + new DriveDistance(-0.5, 10, drivetrain), + new TurnDegrees(-0.5, 180, drivetrain), + new DriveDistance(-0.5, 10, drivetrain), + new TurnDegrees(0.5, 180, drivetrain)); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/AutonomousTime.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/AutonomousTime.java new file mode 100644 index 0000000000..4cc9afa513 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/AutonomousTime.java @@ -0,0 +1,25 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.examples.romireference.commands; + +import edu.wpi.first.wpilibj.examples.romireference.subsystems.Drivetrain; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +public class AutonomousTime extends SequentialCommandGroup { + /** + * Creates a new Autonomous Drive based on time. This will drive out for a period of time, turn + * around for time (equivalent to time to turn around) and drive forward again. This should mimic + * driving out, turning around and driving back. + * + * @param drivetrain The drive subsystem on which this command will run + */ + public AutonomousTime(Drivetrain drivetrain) { + addCommands( + new DriveTime(-0.6, 2.0, drivetrain), + new TurnTime(-0.5, 1.3, drivetrain), + new DriveTime(-0.6, 2.0, drivetrain), + new TurnTime(0.5, 1.3, drivetrain)); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/DriveDistance.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/DriveDistance.java new file mode 100644 index 0000000000..f316ce6279 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/DriveDistance.java @@ -0,0 +1,55 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.examples.romireference.commands; + +import edu.wpi.first.wpilibj.examples.romireference.subsystems.Drivetrain; +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class DriveDistance extends CommandBase { + private final Drivetrain m_drive; + private final double m_distance; + private final double m_speed; + + /** + * Creates a new DriveDistance. This command will drive your your robot for a desired distance at + * a desired speed. + * + * @param speed The speed at which the robot will drive + * @param inches The number of inches the robot will drive + * @param drive The drivetrain subsystem on which this command will run + */ + public DriveDistance(double speed, double inches, Drivetrain drive) { + m_distance = inches; + m_speed = speed; + m_drive = drive; + addRequirements(drive); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + m_drive.arcadeDrive(0, 0); + m_drive.resetEncoders(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + m_drive.arcadeDrive(m_speed, 0); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + m_drive.arcadeDrive(0, 0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + // Compare distance travelled from start to desired distance + return Math.abs(m_drive.getAverageDistanceInch()) >= m_distance; + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/DriveTime.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/DriveTime.java new file mode 100644 index 0000000000..80636f0da8 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/DriveTime.java @@ -0,0 +1,54 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.examples.romireference.commands; + +import edu.wpi.first.wpilibj.examples.romireference.subsystems.Drivetrain; +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class DriveTime extends CommandBase { + private final double m_duration; + private final double m_speed; + private final Drivetrain m_drive; + private long m_startTime; + + /** + * Creates a new DriveTime. This command will drive your robot for a desired speed and time. + * + * @param speed The speed which the robot will drive. Negative is in reverse. + * @param time How much time to drive in seconds + * @param drive The drivetrain subsystem on which this command will run + */ + public DriveTime(double speed, double time, Drivetrain drive) { + m_speed = speed; + m_duration = time * 1000; + m_drive = drive; + addRequirements(drive); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + m_startTime = System.currentTimeMillis(); + m_drive.arcadeDrive(0, 0); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + m_drive.arcadeDrive(m_speed, 0); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + m_drive.arcadeDrive(0, 0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return (System.currentTimeMillis() - m_startTime) >= m_duration; + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/TurnDegrees.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/TurnDegrees.java new file mode 100644 index 0000000000..78ffa6cc86 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/TurnDegrees.java @@ -0,0 +1,68 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.examples.romireference.commands; + +import edu.wpi.first.wpilibj.examples.romireference.subsystems.Drivetrain; +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class TurnDegrees extends CommandBase { + private final Drivetrain m_drive; + private final double m_degrees; + private final double m_speed; + + /** + * Creates a new TurnDegrees. This command will turn your robot for a desired rotation (in + * degrees) and rotational speed. + * + * @param speed The speed which the robot will drive. Negative is in reverse. + * @param degrees Degrees to turn. Leverages encoders to compare distance. + * @param drive The drive subsystem on which this command will run + */ + public TurnDegrees(double speed, double degrees, Drivetrain drive) { + m_degrees = degrees; + m_speed = speed; + m_drive = drive; + addRequirements(drive); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + // Set motors to stop, read encoder values for starting point + m_drive.arcadeDrive(0, 0); + m_drive.resetEncoders(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + m_drive.arcadeDrive(0, m_speed); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + m_drive.arcadeDrive(0, 0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + /* Need to convert distance travelled to degrees. The Standard + Romi Chassis found here, https://www.pololu.com/category/203/romi-chassis-kits, + has a wheel placement diameter (149 mm) - width of the wheel (8 mm) = 141 mm + or 5.551 inches. We then take into consideration the width of the tires. + */ + double inchPerDegree = Math.PI * 5.551 / 360; + // Compare distance travelled from start to distance based on degree turn + return getAverageTurningDistance() >= (inchPerDegree * m_degrees); + } + + private double getAverageTurningDistance() { + double leftDistance = Math.abs(m_drive.getLeftDistanceInch()); + double rightDistance = Math.abs(m_drive.getRightDistanceInch()); + return (leftDistance + rightDistance) / 2.0; + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/TurnTime.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/TurnTime.java new file mode 100644 index 0000000000..10341f9b6e --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/TurnTime.java @@ -0,0 +1,58 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.examples.romireference.commands; + +import edu.wpi.first.wpilibj.examples.romireference.subsystems.Drivetrain; +import edu.wpi.first.wpilibj2.command.CommandBase; + +/* + * Creates a new TurnTime command. This command will turn your robot for a + * desired rotational speed and time. + */ +public class TurnTime extends CommandBase { + private final double m_duration; + private final double m_rotationalSpeed; + private final Drivetrain m_drive; + private long m_startTime; + + /** + * Creates a new TurnTime. + * + * @param speed The speed which the robot will turn. Negative is in reverse. + * @param time How much time to turn in seconds + * @param drive The drive subsystem on which this command will run + */ + public TurnTime(double speed, double time, Drivetrain drive) { + m_rotationalSpeed = speed; + m_duration = time * 1000; + m_drive = drive; + addRequirements(drive); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + m_startTime = System.currentTimeMillis(); + m_drive.arcadeDrive(0, 0); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + m_drive.arcadeDrive(0, m_rotationalSpeed); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + m_drive.arcadeDrive(0, 0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return (System.currentTimeMillis() - m_startTime) >= m_duration; + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/subsystems/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/subsystems/Drivetrain.java index 1537741591..46d7e6c778 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/subsystems/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/subsystems/Drivetrain.java @@ -11,7 +11,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Drivetrain extends SubsystemBase { private static final double kCountsPerRevolution = 1440.0; - private static final double kWheelDiameterInch = 2.75; + private static final double kWheelDiameterInch = 2.75591; // 70 mm // The Romi has the left and right motors set to // PWM channels 0 and 1 respectively @@ -28,10 +28,9 @@ public class Drivetrain extends SubsystemBase { /** Creates a new Drivetrain. */ public Drivetrain() { - // DifferentialDrive defaults to having the right side flipped - // We don't need to do this because the Romi has accounted for this - // in firmware/hardware - m_diffDrive.setRightSideInverted(false); + // Use inches as unit for encoder distances + m_leftEncoder.setDistancePerPulse((Math.PI * kWheelDiameterInch) / kCountsPerRevolution); + m_rightEncoder.setDistancePerPulse((Math.PI * kWheelDiameterInch) / kCountsPerRevolution); resetEncoders(); } @@ -53,11 +52,15 @@ public class Drivetrain extends SubsystemBase { } public double getLeftDistanceInch() { - return Math.PI * kWheelDiameterInch * (getLeftEncoderCount() / kCountsPerRevolution); + return m_leftEncoder.getDistance(); } public double getRightDistanceInch() { - return Math.PI * kWheelDiameterInch * (getRightEncoderCount() / kCountsPerRevolution); + return m_rightEncoder.getDistance(); + } + + public double getAverageDistanceInch() { + return (getLeftDistanceInch() + getRightDistanceInch()) / 2.0; } @Override diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romicommandbased/subsystems/RomiDrivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romicommandbased/subsystems/RomiDrivetrain.java index b9f9458b58..3d256e0cf3 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romicommandbased/subsystems/RomiDrivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romicommandbased/subsystems/RomiDrivetrain.java @@ -11,7 +11,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; public class RomiDrivetrain extends SubsystemBase { private static final double kCountsPerRevolution = 1440.0; - private static final double kWheelDiameterInch = 2.75; + private static final double kWheelDiameterInch = 2.75591; // 70 mm // The Romi has the left and right motors set to // PWM channels 0 and 1 respectively @@ -28,10 +28,9 @@ public class RomiDrivetrain extends SubsystemBase { /** Creates a new RomiDrivetrain. */ public RomiDrivetrain() { - // DifferentialDrive defaults to having the right side flipped - // We don't need to do this because the Romi has accounted for this - // in firmware/hardware - m_diffDrive.setRightSideInverted(false); + // Use inches as unit for encoder distances + m_leftEncoder.setDistancePerPulse((Math.PI * kWheelDiameterInch) / kCountsPerRevolution); + m_rightEncoder.setDistancePerPulse((Math.PI * kWheelDiameterInch) / kCountsPerRevolution); resetEncoders(); } @@ -44,20 +43,12 @@ public class RomiDrivetrain extends SubsystemBase { m_rightEncoder.reset(); } - public int getLeftEncoderCount() { - return m_leftEncoder.get(); - } - - public int getRightEncoderCount() { - return m_rightEncoder.get(); - } - public double getLeftDistanceInch() { - return Math.PI * kWheelDiameterInch * (getLeftEncoderCount() / kCountsPerRevolution); + return m_leftEncoder.getDistance(); } public double getRightDistanceInch() { - return Math.PI * kWheelDiameterInch * (getRightEncoderCount() / kCountsPerRevolution); + return m_rightEncoder.getDistance(); } @Override diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romitimed/RomiDrivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romitimed/RomiDrivetrain.java index 12f86b3cff..84d627280a 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romitimed/RomiDrivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romitimed/RomiDrivetrain.java @@ -10,7 +10,7 @@ import edu.wpi.first.wpilibj.drive.DifferentialDrive; public class RomiDrivetrain { private static final double kCountsPerRevolution = 1440.0; - private static final double kWheelDiameterInch = 2.75; + private static final double kWheelDiameterInch = 2.75591; // 70 mm // The Romi has the left and right motors set to // PWM channels 0 and 1 respectively @@ -27,10 +27,9 @@ public class RomiDrivetrain { /** Creates a new RomiDrivetrain. */ public RomiDrivetrain() { - // DifferentialDrive defaults to having the right side flipped - // We don't need to do this because the Romi has accounted for this - // in firmware/hardware - m_diffDrive.setRightSideInverted(false); + // Use inches as unit for encoder distances + m_leftEncoder.setDistancePerPulse((Math.PI * kWheelDiameterInch) / kCountsPerRevolution); + m_rightEncoder.setDistancePerPulse((Math.PI * kWheelDiameterInch) / kCountsPerRevolution); resetEncoders(); } @@ -43,19 +42,11 @@ public class RomiDrivetrain { m_rightEncoder.reset(); } - public int getLeftEncoderCount() { - return m_leftEncoder.get(); - } - - public int getRightEncoderCount() { - return m_rightEncoder.get(); - } - public double getLeftDistanceInch() { - return Math.PI * kWheelDiameterInch * (getLeftEncoderCount() / kCountsPerRevolution); + return m_leftEncoder.getDistance(); } public double getRightDistanceInch() { - return Math.PI * kWheelDiameterInch * (getRightEncoderCount() / kCountsPerRevolution); + return m_rightEncoder.getDistance(); } }