[examples] Update Romi template and example (#2996)

Updated the RomiReference example to have autonomous example.
Updated RomiReference and both Romi templates to use Encoder.getDistance().
Removed motor inversion.

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();

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<Command> 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));
   }
 }

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<Double> m_xaxisSpeedSupplier;
   private final Supplier<Double> 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<Double> xaxisSpeedSupplier,
       Supplier<Double> zaxisRotateSuppplier) {
     m_drivetrain = drivetrain;
     m_xaxisSpeedSupplier = xaxisSpeedSupplier;
     m_zaxisRotateSupplier = zaxisRotateSuppplier;
-
     addRequirements(drivetrain);
   }
 

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));
+  }
+}

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));
+  }
+}

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;
+  }
+}

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;
+  }
+}

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;
+  }
+}

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;
+  }
+}

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

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

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();
   }
 }