[examples] Add Java Examples and Templates for the XRP (#5529)

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/templates.json

@@ -81,6 +81,30 @@
     "mainclass": "Main",
     "commandversion": 2
   },
+  {
+    "name": "XRP - Command Robot",
+    "description": "XRP - Command style",
+    "tags": [
+      "Command",
+      "XRP"
+    ],
+    "foldername": "xrpcommandbased",
+    "gradlebase": "javaxrp",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "XRP - Timed Robot",
+    "description": "XRP - Timed style",
+    "tags": [
+      "Timed",
+      "XRP"
+    ],
+    "foldername": "xrptimed",
+    "gradlebase": "javaxrp",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
   {
     "name": "Educational Robot",
     "description": "Educational Robot - Not for competition use",
@@ -103,5 +127,17 @@
     "gradlebase": "javaromi",
     "mainclass": "Main",
     "commandversion": 2
+  },
+  {
+    "name": "XRP - Educational Robot",
+    "description": "XRP - Educational Robot",
+    "tags": [
+      "Educational",
+      "XRP"
+    ],
+    "foldername": "xrpeducational",
+    "gradlebase": "javaxrp",
+    "mainclass": "Main",
+    "commandversion": 2
   }
 ]

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpcommandbased/Constants.java

@@ -0,0 +1,15 @@
+// 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.templates.xrpcommandbased;
+
+/**
+ * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
+ * constants. This class should not be used for any other purpose. All constants should be declared
+ * globally (i.e. public static). Do not put anything functional in this class.
+ *
+ * <p>It is advised to statically import this class (or one of its inner classes) wherever the
+ * constants are needed, to reduce verbosity.
+ */
+public final class Constants {}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpcommandbased/Main.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.templates.xrpcommandbased;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpcommandbased/Robot.java

@@ -0,0 +1,95 @@
+// 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.templates.xrpcommandbased;
+
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+
+/**
+ * The VM is configured to automatically run this class, and to call the functions corresponding to
+ * each mode, as described in the TimedRobot documentation. If you change the name of this class or
+ * the package after creating this project, you must also update the build.gradle file in the
+ * project.
+ */
+public class Robot extends TimedRobot {
+  private Command m_autonomousCommand;
+
+  private RobotContainer m_robotContainer;
+
+  /**
+   * This function is run when the robot is first started up and should be used for any
+   * initialization code.
+   */
+  @Override
+  public void robotInit() {
+    // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
+    // autonomous chooser on the dashboard.
+    m_robotContainer = new RobotContainer();
+  }
+
+  /**
+   * This function is called every 20 ms, no matter the mode. Use this for items like diagnostics
+   * that you want ran during disabled, autonomous, teleoperated and test.
+   *
+   * <p>This runs after the mode specific periodic functions, but before LiveWindow and
+   * SmartDashboard integrated updating.
+   */
+  @Override
+  public void robotPeriodic() {
+    // Runs the Scheduler.  This is responsible for polling buttons, adding newly-scheduled
+    // commands, running already-scheduled commands, removing finished or interrupted commands,
+    // and running subsystem periodic() methods.  This must be called from the robot's periodic
+    // block in order for anything in the Command-based framework to work.
+    CommandScheduler.getInstance().run();
+  }
+
+  /** This function is called once each time the robot enters Disabled mode. */
+  @Override
+  public void disabledInit() {}
+
+  @Override
+  public void disabledPeriodic() {}
+
+  /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
+  @Override
+  public void autonomousInit() {
+    m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+
+    // schedule the autonomous command (example)
+    if (m_autonomousCommand != null) {
+      m_autonomousCommand.schedule();
+    }
+  }
+
+  /** This function is called periodically during autonomous. */
+  @Override
+  public void autonomousPeriodic() {}
+
+  @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 line or comment it out.
+    if (m_autonomousCommand != null) {
+      m_autonomousCommand.cancel();
+    }
+  }
+
+  /** This function is called periodically during operator control. */
+  @Override
+  public void teleopPeriodic() {}
+
+  @Override
+  public void testInit() {
+    // Cancels all running commands at the start of test mode.
+    CommandScheduler.getInstance().cancelAll();
+  }
+
+  /** This function is called periodically during test mode. */
+  @Override
+  public void testPeriodic() {}
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpcommandbased/RobotContainer.java

@@ -0,0 +1,47 @@
+// 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.templates.xrpcommandbased;
+
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.templates.xrpcommandbased.commands.ExampleCommand;
+import edu.wpi.first.wpilibj.templates.xrpcommandbased.subsystems.XRPDrivetrain;
+import edu.wpi.first.wpilibj2.command.Command;
+
+/**
+ * This class is where the bulk of the robot should be declared. Since Command-based is a
+ * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
+ * subsystems, commands, and button mappings) should be declared here.
+ */
+public class RobotContainer {
+  // The robot's subsystems and commands are defined here...
+  private final XRPDrivetrain m_xrpDrivetrain = new XRPDrivetrain();
+
+  private final ExampleCommand m_autoCommand = new ExampleCommand(m_xrpDrivetrain);
+
+  /** The container for the robot. Contains subsystems, OI devices, and commands. */
+  public RobotContainer() {
+    // Configure the button bindings
+    configureButtonBindings();
+  }
+
+  /**
+   * Use this method to define your button->command mappings. Buttons can be created by
+   * instantiating a {@link edu.wpi.first.wpilibj.GenericHID} or one of its subclasses ({@link
+   * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link
+   * edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+   */
+  private void configureButtonBindings() {}
+
+  /**
+   * Use this to pass the autonomous command to the main {@link Robot} class.
+   *
+   * @return the command to run in autonomous
+   */
+  public Command getAutonomousCommand() {
+    // An ExampleCommand will run in autonomous
+    return m_autoCommand;
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpcommandbased/commands/ExampleCommand.java

@@ -0,0 +1,43 @@
+// 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.templates.xrpcommandbased.commands;
+
+import edu.wpi.first.wpilibj.templates.xrpcommandbased.subsystems.XRPDrivetrain;
+import edu.wpi.first.wpilibj2.command.Command;
+
+/** An example command that uses an example subsystem. */
+public class ExampleCommand extends Command {
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+  private final XRPDrivetrain m_subsystem;
+
+  /**
+   * Creates a new ExampleCommand.
+   *
+   * @param subsystem The subsystem used by this command.
+   */
+  public ExampleCommand(XRPDrivetrain subsystem) {
+    m_subsystem = subsystem;
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(subsystem);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {}
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpcommandbased/devices/XRPMotor.java

@@ -0,0 +1,109 @@
+// 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.templates.xrpcommandbased.devices;
+
+import edu.wpi.first.hal.SimBoolean;
+import edu.wpi.first.hal.SimDevice;
+import edu.wpi.first.hal.SimDevice.Direction;
+import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.wpilibj.motorcontrol.MotorController;
+import java.util.HashMap;
+import java.util.HashSet;
+
+/**
+ * XRPMotor.
+ *
+ * <p>A SimDevice based motor controller representing the motors on an XRP robot
+ */
+public class XRPMotor implements MotorController {
+  private static HashMap<Integer, String> s_simDeviceNameMap = new HashMap<>();
+  private static HashSet<Integer> s_registeredDevices = new HashSet<>();
+
+  private static void checkDeviceAllocation(int deviceNum) {
+    if (!s_simDeviceNameMap.containsKey(deviceNum)) {
+      throw new IllegalArgumentException("Invalid XRPMotor device number. Should be 0-3");
+    }
+
+    if (s_registeredDevices.contains(deviceNum)) {
+      throw new IllegalArgumentException("XRPMotor " + deviceNum + " already allocated");
+    }
+
+    s_registeredDevices.add(deviceNum);
+  }
+
+  static {
+    s_simDeviceNameMap.put(0, "motorL");
+    s_simDeviceNameMap.put(1, "motorR");
+    s_simDeviceNameMap.put(2, "motor3");
+    s_simDeviceNameMap.put(3, "motor4");
+  }
+
+  private final SimDouble m_simSpeed;
+  private final SimBoolean m_simInverted;
+
+  /** XRPMotor. */
+  public XRPMotor(int deviceNum) {
+    checkDeviceAllocation(deviceNum);
+
+    // We want this to appear on the WS messages as type: "XRPMotor", device: <motor name>
+    String simDeviceName = "XRPMotor:" + s_simDeviceNameMap.get(deviceNum);
+    SimDevice xrpMotorSimDevice = SimDevice.create(simDeviceName);
+
+    if (xrpMotorSimDevice != null) {
+      xrpMotorSimDevice.createBoolean("init", Direction.kOutput, true);
+      m_simInverted = xrpMotorSimDevice.createBoolean("inverted", Direction.kInput, false);
+      m_simSpeed = xrpMotorSimDevice.createDouble("speed", Direction.kOutput, 0.0);
+    } else {
+      m_simInverted = null;
+      m_simSpeed = null;
+    }
+  }
+
+  @Override
+  public void set(double speed) {
+    if (m_simSpeed != null) {
+      boolean invert = false;
+      if (m_simInverted != null) {
+        invert = m_simInverted.get();
+      }
+
+      m_simSpeed.set(invert ? -speed : speed);
+    }
+  }
+
+  @Override
+  public double get() {
+    if (m_simSpeed != null) {
+      return m_simSpeed.get();
+    }
+
+    return 0.0;
+  }
+
+  @Override
+  public void setInverted(boolean isInverted) {
+    if (m_simInverted != null) {
+      m_simInverted.set(isInverted);
+    }
+  }
+
+  @Override
+  public boolean getInverted() {
+    if (m_simInverted != null) {
+      return m_simInverted.get();
+    }
+    return false;
+  }
+
+  @Override
+  public void disable() {
+    set(0.0);
+  }
+
+  @Override
+  public void stopMotor() {
+    set(0.0);
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpcommandbased/devices/XRPServo.java

@@ -0,0 +1,123 @@
+// 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.templates.xrpcommandbased.devices;
+
+import edu.wpi.first.hal.SimDevice;
+import edu.wpi.first.hal.SimDevice.Direction;
+import edu.wpi.first.hal.SimDouble;
+import java.util.HashMap;
+import java.util.HashSet;
+
+/**
+ * XRPServo.
+ *
+ * <p>A SimDevice based servo
+ */
+public class XRPServo {
+  private static HashMap<Integer, String> s_simDeviceNameMap = new HashMap<>();
+  private static HashSet<Integer> s_registeredDevices = new HashSet<>();
+
+  private static void checkDeviceAllocation(int deviceNum) {
+    if (!s_simDeviceNameMap.containsKey(deviceNum)) {
+      throw new IllegalArgumentException("Invalid XRPServo device number. Should be 4-5");
+    }
+
+    if (s_registeredDevices.contains(deviceNum)) {
+      throw new IllegalArgumentException("XRPServo " + deviceNum + " already allocated");
+    }
+
+    s_registeredDevices.add(deviceNum);
+  }
+
+  static {
+    s_simDeviceNameMap.put(4, "servo1");
+    s_simDeviceNameMap.put(5, "servo2");
+  }
+
+  private final SimDouble m_simPosition;
+
+  /** XRPServo. */
+  public XRPServo(int deviceNum) {
+    checkDeviceAllocation(deviceNum);
+
+    // We want this to appear on WS as type: "XRPServo", device: <servo name>
+    String simDeviceName = "XRPServo:" + s_simDeviceNameMap.get(deviceNum);
+    SimDevice xrpServoSimDevice = SimDevice.create(simDeviceName);
+
+    if (xrpServoSimDevice != null) {
+      xrpServoSimDevice.createBoolean("init", Direction.kOutput, true);
+      // This should mimic PWM position [0.0, 1.0]
+      m_simPosition = xrpServoSimDevice.createDouble("position", Direction.kOutput, 0.5);
+    } else {
+      m_simPosition = null;
+    }
+  }
+
+  /**
+   * Set the servo angle.
+   *
+   * @param angle Desired angle in degrees
+   */
+  public void setAngle(double angle) {
+    if (angle < 0.0) {
+      angle = 0.0;
+    }
+
+    if (angle > 180.0) {
+      angle = 180.0;
+    }
+
+    double pos = angle / 180.0;
+
+    if (m_simPosition != null) {
+      m_simPosition.set(pos);
+    }
+  }
+
+  /**
+   * Get the servo angle.
+   *
+   * @return Current servo angle
+   */
+  public double getAngle() {
+    if (m_simPosition != null) {
+      return m_simPosition.get() * 180.0;
+    }
+
+    return 90.0;
+  }
+
+  /**
+   * Set the servo position.
+   *
+   * @param pos Desired position (Between 0.0 and 1.0)
+   */
+  public void setPosition(double pos) {
+    if (pos < 0.0) {
+      pos = 0.0;
+    }
+
+    if (pos > 1.0) {
+      pos = 1.0;
+    }
+
+    if (m_simPosition != null) {
+      m_simPosition.set(pos);
+    }
+  }
+
+  /**
+   * Get the servo position.
+   *
+   * @return Current servo position
+   */
+  public double getPosition() {
+    if (m_simPosition != null) {
+      return m_simPosition.get();
+    }
+
+    return 0.5;
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpcommandbased/subsystems/XRPDrivetrain.java

@@ -0,0 +1,69 @@
+// 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.templates.xrpcommandbased.subsystems;
+
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.templates.xrpcommandbased.devices.XRPMotor;
+import edu.wpi.first.wpilibj2.command.Subsystem;
+
+public class XRPDrivetrain extends Subsystem {
+  private static final double kGearRatio =
+      (30.0 / 14.0) * (28.0 / 16.0) * (36.0 / 9.0) * (26.0 / 8.0); // 48.75:1
+  private static final double kCountsPerMotorShaftRev = 12.0;
+  private static final double kCountsPerRevolution = kCountsPerMotorShaftRev * kGearRatio; // 585.0
+  private static final double kWheelDiameterInch = 2.3622; // 60 mm
+
+  // The XRP has the left and right motors set to
+  // channels 0 and 1 respectively
+  private final XRPMotor m_leftMotor = new XRPMotor(0);
+  private final XRPMotor m_rightMotor = new XRPMotor(1);
+
+  // The XRP has onboard encoders that are hardcoded
+  // to use DIO pins 4/5 and 6/7 for the left and right
+  private final Encoder m_leftEncoder = new Encoder(4, 5);
+  private final Encoder m_rightEncoder = new Encoder(6, 7);
+
+  // Set up the differential drive controller
+  private final DifferentialDrive m_diffDrive = new DifferentialDrive(m_leftMotor, m_rightMotor);
+
+  /** Creates a new XRPDrivetrain. */
+  public XRPDrivetrain() {
+    // Use inches as unit for encoder distances
+    m_leftEncoder.setDistancePerPulse((Math.PI * kWheelDiameterInch) / kCountsPerRevolution);
+    m_rightEncoder.setDistancePerPulse((Math.PI * kWheelDiameterInch) / kCountsPerRevolution);
+    resetEncoders();
+
+    // Invert right side since motor is flipped
+    m_rightMotor.setInverted(true);
+  }
+
+  public void arcadeDrive(double xaxisSpeed, double zaxisRotate) {
+    m_diffDrive.arcadeDrive(xaxisSpeed, zaxisRotate);
+  }
+
+  public void resetEncoders() {
+    m_leftEncoder.reset();
+    m_rightEncoder.reset();
+  }
+
+  public double getLeftDistanceInch() {
+    return m_leftEncoder.getDistance();
+  }
+
+  public double getRightDistanceInch() {
+    return m_rightEncoder.getDistance();
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+
+  @Override
+  public void simulationPeriodic() {
+    // This method will be called once per scheduler run during simulation
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpeducational/EducationalRobot.java

@@ -0,0 +1,104 @@
+// 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.templates.xrpeducational;
+
+import edu.wpi.first.hal.DriverStationJNI;
+import edu.wpi.first.util.WPIUtilJNI;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.RobotBase;
+import edu.wpi.first.wpilibj.internal.DriverStationModeThread;
+
+/** Educational robot base class. */
+public class EducationalRobot extends RobotBase {
+  public void robotInit() {}
+
+  public void disabled() {}
+
+  public void run() {}
+
+  public void autonomous() {
+    run();
+  }
+
+  public void teleop() {
+    run();
+  }
+
+  public void test() {
+    run();
+  }
+
+  private volatile boolean m_exit;
+
+  @Override
+  public void startCompetition() {
+    robotInit();
+
+    DriverStationModeThread modeThread = new DriverStationModeThread();
+
+    int event = WPIUtilJNI.createEvent(false, false);
+
+    DriverStation.provideRefreshedDataEventHandle(event);
+
+    // Tell the DS that the robot is ready to be enabled
+    DriverStationJNI.observeUserProgramStarting();
+
+    while (!Thread.currentThread().isInterrupted() && !m_exit) {
+      if (isDisabled()) {
+        modeThread.inDisabled(true);
+        disabled();
+        modeThread.inDisabled(false);
+        while (isDisabled()) {
+          try {
+            WPIUtilJNI.waitForObject(event);
+          } catch (InterruptedException e) {
+            Thread.currentThread().interrupt();
+          }
+        }
+      } else if (isAutonomous()) {
+        modeThread.inAutonomous(true);
+        autonomous();
+        modeThread.inAutonomous(false);
+        while (isAutonomousEnabled()) {
+          try {
+            WPIUtilJNI.waitForObject(event);
+          } catch (InterruptedException e) {
+            Thread.currentThread().interrupt();
+          }
+        }
+      } else if (isTest()) {
+        modeThread.inTest(true);
+        test();
+        modeThread.inTest(false);
+        while (isTest() && isEnabled()) {
+          try {
+            WPIUtilJNI.waitForObject(event);
+          } catch (InterruptedException e) {
+            Thread.currentThread().interrupt();
+          }
+        }
+      } else {
+        modeThread.inTeleop(true);
+        teleop();
+        modeThread.inTeleop(false);
+        while (isTeleopEnabled()) {
+          try {
+            WPIUtilJNI.waitForObject(event);
+          } catch (InterruptedException e) {
+            Thread.currentThread().interrupt();
+          }
+        }
+      }
+    }
+
+    DriverStation.removeRefreshedDataEventHandle(event);
+    modeThread.close();
+  }
+
+  @Override
+  public void endCompetition() {
+    m_exit = true;
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpeducational/Main.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.templates.xrpeducational;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpeducational/Robot.java

@@ -0,0 +1,23 @@
+// 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.templates.xrpeducational;
+
+/**
+ * The VM is configured to automatically run this class, and to call the run() function when the
+ * robot is enabled. If you change the name of this class or the package after creating this
+ * project, you must also update the build.gradle file in the project.
+ */
+public class Robot extends EducationalRobot {
+  /**
+   * This function is run when the robot is first started up and should be used for any
+   * initialization code.
+   */
+  @Override
+  public void robotInit() {}
+
+  /** This function is run when the robot is enabled. */
+  @Override
+  public void run() {}
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpeducational/XRPDrivetrain.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.templates.xrpeducational;
+
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.templates.xrpeducational.devices.XRPMotor;
+
+public class XRPDrivetrain {
+  private static final double kGearRatio =
+      (30.0 / 14.0) * (28.0 / 16.0) * (36.0 / 9.0) * (26.0 / 8.0); // 48.75:1
+  private static final double kCountsPerMotorShaftRev = 12.0;
+  private static final double kCountsPerRevolution = kCountsPerMotorShaftRev * kGearRatio; // 585.0
+  private static final double kWheelDiameterInch = 2.3622; // 60 mm
+
+  // The XRP has the left and right motors set to
+  // channels 0 and 1 respectively
+  private final XRPMotor m_leftMotor = new XRPMotor(0);
+  private final XRPMotor m_rightMotor = new XRPMotor(1);
+
+  // The XRP has onboard encoders that are hardcoded
+  // to use DIO pins 4/5 and 6/7 for the left and right
+  private final Encoder m_leftEncoder = new Encoder(4, 5);
+  private final Encoder m_rightEncoder = new Encoder(6, 7);
+
+  // Set up the differential drive controller
+  private final DifferentialDrive m_diffDrive = new DifferentialDrive(m_leftMotor, m_rightMotor);
+
+  /** Creates a new XRPDrivetrain. */
+  public XRPDrivetrain() {
+    // Use inches as unit for encoder distances
+    m_leftEncoder.setDistancePerPulse((Math.PI * kWheelDiameterInch) / kCountsPerRevolution);
+    m_rightEncoder.setDistancePerPulse((Math.PI * kWheelDiameterInch) / kCountsPerRevolution);
+    resetEncoders();
+
+    // Invert right side since motor is flipped
+    m_rightMotor.setInverted(true);
+  }
+
+  public void arcadeDrive(double xaxisSpeed, double zaxisRotate) {
+    m_diffDrive.arcadeDrive(xaxisSpeed, zaxisRotate);
+  }
+
+  public void resetEncoders() {
+    m_leftEncoder.reset();
+    m_rightEncoder.reset();
+  }
+
+  public double getLeftDistanceInch() {
+    return m_leftEncoder.getDistance();
+  }
+
+  public double getRightDistanceInch() {
+    return m_rightEncoder.getDistance();
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpeducational/devices/XRPMotor.java

@@ -0,0 +1,109 @@
+// 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.templates.xrpeducational.devices;
+
+import edu.wpi.first.hal.SimBoolean;
+import edu.wpi.first.hal.SimDevice;
+import edu.wpi.first.hal.SimDevice.Direction;
+import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.wpilibj.motorcontrol.MotorController;
+import java.util.HashMap;
+import java.util.HashSet;
+
+/**
+ * XRPMotor.
+ *
+ * <p>A SimDevice based motor controller representing the motors on an XRP robot
+ */
+public class XRPMotor implements MotorController {
+  private static HashMap<Integer, String> s_simDeviceNameMap = new HashMap<>();
+  private static HashSet<Integer> s_registeredDevices = new HashSet<>();
+
+  private static void checkDeviceAllocation(int deviceNum) {
+    if (!s_simDeviceNameMap.containsKey(deviceNum)) {
+      throw new IllegalArgumentException("Invalid XRPMotor device number. Should be 0-3");
+    }
+
+    if (s_registeredDevices.contains(deviceNum)) {
+      throw new IllegalArgumentException("XRPMotor " + deviceNum + " already allocated");
+    }
+
+    s_registeredDevices.add(deviceNum);
+  }
+
+  static {
+    s_simDeviceNameMap.put(0, "motorL");
+    s_simDeviceNameMap.put(1, "motorR");
+    s_simDeviceNameMap.put(2, "motor3");
+    s_simDeviceNameMap.put(3, "motor4");
+  }
+
+  private final SimDouble m_simSpeed;
+  private final SimBoolean m_simInverted;
+
+  /** XRPMotor. */
+  public XRPMotor(int deviceNum) {
+    checkDeviceAllocation(deviceNum);
+
+    // We want this to appear on the WS messages as type: "XRPMotor", device: <motor name>
+    String simDeviceName = "XRPMotor:" + s_simDeviceNameMap.get(deviceNum);
+    SimDevice xrpMotorSimDevice = SimDevice.create(simDeviceName);
+
+    if (xrpMotorSimDevice != null) {
+      xrpMotorSimDevice.createBoolean("init", Direction.kOutput, true);
+      m_simInverted = xrpMotorSimDevice.createBoolean("inverted", Direction.kInput, false);
+      m_simSpeed = xrpMotorSimDevice.createDouble("speed", Direction.kOutput, 0.0);
+    } else {
+      m_simInverted = null;
+      m_simSpeed = null;
+    }
+  }
+
+  @Override
+  public void set(double speed) {
+    if (m_simSpeed != null) {
+      boolean invert = false;
+      if (m_simInverted != null) {
+        invert = m_simInverted.get();
+      }
+
+      m_simSpeed.set(invert ? -speed : speed);
+    }
+  }
+
+  @Override
+  public double get() {
+    if (m_simSpeed != null) {
+      return m_simSpeed.get();
+    }
+
+    return 0.0;
+  }
+
+  @Override
+  public void setInverted(boolean isInverted) {
+    if (m_simInverted != null) {
+      m_simInverted.set(isInverted);
+    }
+  }
+
+  @Override
+  public boolean getInverted() {
+    if (m_simInverted != null) {
+      return m_simInverted.get();
+    }
+    return false;
+  }
+
+  @Override
+  public void disable() {
+    set(0.0);
+  }
+
+  @Override
+  public void stopMotor() {
+    set(0.0);
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrpeducational/devices/XRPServo.java

@@ -0,0 +1,123 @@
+// 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.templates.xrpeducational.devices;
+
+import edu.wpi.first.hal.SimDevice;
+import edu.wpi.first.hal.SimDevice.Direction;
+import edu.wpi.first.hal.SimDouble;
+import java.util.HashMap;
+import java.util.HashSet;
+
+/**
+ * XRPServo.
+ *
+ * <p>A SimDevice based servo
+ */
+public class XRPServo {
+  private static HashMap<Integer, String> s_simDeviceNameMap = new HashMap<>();
+  private static HashSet<Integer> s_registeredDevices = new HashSet<>();
+
+  private static void checkDeviceAllocation(int deviceNum) {
+    if (!s_simDeviceNameMap.containsKey(deviceNum)) {
+      throw new IllegalArgumentException("Invalid XRPServo device number. Should be 4-5");
+    }
+
+    if (s_registeredDevices.contains(deviceNum)) {
+      throw new IllegalArgumentException("XRPServo " + deviceNum + " already allocated");
+    }
+
+    s_registeredDevices.add(deviceNum);
+  }
+
+  static {
+    s_simDeviceNameMap.put(4, "servo1");
+    s_simDeviceNameMap.put(5, "servo2");
+  }
+
+  private final SimDouble m_simPosition;
+
+  /** XRPServo. */
+  public XRPServo(int deviceNum) {
+    checkDeviceAllocation(deviceNum);
+
+    // We want this to appear on WS as type: "XRPServo", device: <servo name>
+    String simDeviceName = "XRPServo:" + s_simDeviceNameMap.get(deviceNum);
+    SimDevice xrpServoSimDevice = SimDevice.create(simDeviceName);
+
+    if (xrpServoSimDevice != null) {
+      xrpServoSimDevice.createBoolean("init", Direction.kOutput, true);
+      // This should mimic PWM position [0.0, 1.0]
+      m_simPosition = xrpServoSimDevice.createDouble("position", Direction.kOutput, 0.5);
+    } else {
+      m_simPosition = null;
+    }
+  }
+
+  /**
+   * Set the servo angle.
+   *
+   * @param angle Desired angle in degrees
+   */
+  public void setAngle(double angle) {
+    if (angle < 0.0) {
+      angle = 0.0;
+    }
+
+    if (angle > 180.0) {
+      angle = 180.0;
+    }
+
+    double pos = angle / 180.0;
+
+    if (m_simPosition != null) {
+      m_simPosition.set(pos);
+    }
+  }
+
+  /**
+   * Get the servo angle.
+   *
+   * @return Current servo angle
+   */
+  public double getAngle() {
+    if (m_simPosition != null) {
+      return m_simPosition.get() * 180.0;
+    }
+
+    return 90.0;
+  }
+
+  /**
+   * Set the servo position.
+   *
+   * @param pos Desired position (Between 0.0 and 1.0)
+   */
+  public void setPosition(double pos) {
+    if (pos < 0.0) {
+      pos = 0.0;
+    }
+
+    if (pos > 1.0) {
+      pos = 1.0;
+    }
+
+    if (m_simPosition != null) {
+      m_simPosition.set(pos);
+    }
+  }
+
+  /**
+   * Get the servo position.
+   *
+   * @return Current servo position
+   */
+  public double getPosition() {
+    if (m_simPosition != null) {
+      return m_simPosition.get();
+    }
+
+    return 0.5;
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrptimed/Main.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.templates.xrptimed;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrptimed/Robot.java

@@ -0,0 +1,102 @@
+// 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.templates.xrptimed;
+
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ * The VM is configured to automatically run this class, and to call the functions corresponding to
+ * each mode, as described in the TimedRobot documentation. If you change the name of this class or
+ * the package after creating this project, you must also update the build.gradle file in the
+ * project.
+ */
+public class Robot extends TimedRobot {
+  private static final String kDefaultAuto = "Default";
+  private static final String kCustomAuto = "My Auto";
+  private String m_autoSelected;
+  private final SendableChooser<String> m_chooser = new SendableChooser<>();
+
+  private final XRPDrivetrain m_drivetrain = new XRPDrivetrain();
+
+  /**
+   * This function is run when the robot is first started up and should be used for any
+   * initialization code.
+   */
+  @Override
+  public void robotInit() {
+    m_chooser.setDefaultOption("Default Auto", kDefaultAuto);
+    m_chooser.addOption("My Auto", kCustomAuto);
+    SmartDashboard.putData("Auto choices", m_chooser);
+  }
+
+  /**
+   * This function is called every 20 ms, no matter the mode. Use this for items like diagnostics
+   * that you want ran during disabled, autonomous, teleoperated and test.
+   *
+   * <p>This runs after the mode specific periodic functions, but before LiveWindow and
+   * SmartDashboard integrated updating.
+   */
+  @Override
+  public void robotPeriodic() {}
+
+  /**
+   * This autonomous (along with the chooser code above) shows how to select between different
+   * autonomous modes using the dashboard. The sendable chooser code works with the Java
+   * SmartDashboard. If you prefer the LabVIEW Dashboard, remove all of the chooser code and
+   * uncomment the getString line to get the auto name from the text box below the Gyro
+   *
+   * <p>You can add additional auto modes by adding additional comparisons to the switch structure
+   * below with additional strings. If using the SendableChooser make sure to add them to the
+   * chooser code above as well.
+   */
+  @Override
+  public void autonomousInit() {
+    m_autoSelected = m_chooser.getSelected();
+    // m_autoSelected = SmartDashboard.getString("Auto Selector", kDefaultAuto);
+    System.out.println("Auto selected: " + m_autoSelected);
+
+    m_drivetrain.resetEncoders();
+  }
+
+  /** This function is called periodically during autonomous. */
+  @Override
+  public void autonomousPeriodic() {
+    switch (m_autoSelected) {
+      case kCustomAuto:
+        // Put custom auto code here
+        break;
+      case kDefaultAuto:
+      default:
+        // Put default auto code here
+        break;
+    }
+  }
+
+  /** This function is called once when teleop is enabled. */
+  @Override
+  public void teleopInit() {}
+
+  /** This function is called periodically during operator control. */
+  @Override
+  public void teleopPeriodic() {}
+
+  /** This function is called once when the robot is disabled. */
+  @Override
+  public void disabledInit() {}
+
+  /** This function is called periodically when disabled. */
+  @Override
+  public void disabledPeriodic() {}
+
+  /** This function is called once when test mode is enabled. */
+  @Override
+  public void testInit() {}
+
+  /** This function is called periodically during test mode. */
+  @Override
+  public void testPeriodic() {}
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrptimed/XRPDrivetrain.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.templates.xrptimed;
+
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.templates.xrptimed.devices.XRPMotor;
+
+public class XRPDrivetrain {
+  private static final double kGearRatio =
+      (30.0 / 14.0) * (28.0 / 16.0) * (36.0 / 9.0) * (26.0 / 8.0); // 48.75:1
+  private static final double kCountsPerMotorShaftRev = 12.0;
+  private static final double kCountsPerRevolution = kCountsPerMotorShaftRev * kGearRatio; // 585.0
+  private static final double kWheelDiameterInch = 2.3622; // 60 mm
+
+  // The XRP has the left and right motors set to
+  // channels 0 and 1 respectively
+  private final XRPMotor m_leftMotor = new XRPMotor(0);
+  private final XRPMotor m_rightMotor = new XRPMotor(1);
+
+  // The XRP has onboard encoders that are hardcoded
+  // to use DIO pins 4/5 and 6/7 for the left and right
+  private final Encoder m_leftEncoder = new Encoder(4, 5);
+  private final Encoder m_rightEncoder = new Encoder(6, 7);
+
+  // Set up the differential drive controller
+  private final DifferentialDrive m_diffDrive = new DifferentialDrive(m_leftMotor, m_rightMotor);
+
+  /** Creates a new XRPDrivetrain. */
+  public XRPDrivetrain() {
+    // Use inches as unit for encoder distances
+    m_leftEncoder.setDistancePerPulse((Math.PI * kWheelDiameterInch) / kCountsPerRevolution);
+    m_rightEncoder.setDistancePerPulse((Math.PI * kWheelDiameterInch) / kCountsPerRevolution);
+    resetEncoders();
+
+    // Invert right side since motor is flipped
+    m_rightMotor.setInverted(true);
+  }
+
+  public void arcadeDrive(double xaxisSpeed, double zaxisRotate) {
+    m_diffDrive.arcadeDrive(xaxisSpeed, zaxisRotate);
+  }
+
+  public void resetEncoders() {
+    m_leftEncoder.reset();
+    m_rightEncoder.reset();
+  }
+
+  public double getLeftDistanceInch() {
+    return m_leftEncoder.getDistance();
+  }
+
+  public double getRightDistanceInch() {
+    return m_rightEncoder.getDistance();
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrptimed/devices/XRPMotor.java

@@ -0,0 +1,109 @@
+// 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.templates.xrptimed.devices;
+
+import edu.wpi.first.hal.SimBoolean;
+import edu.wpi.first.hal.SimDevice;
+import edu.wpi.first.hal.SimDevice.Direction;
+import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.wpilibj.motorcontrol.MotorController;
+import java.util.HashMap;
+import java.util.HashSet;
+
+/**
+ * XRPMotor.
+ *
+ * <p>A SimDevice based motor controller representing the motors on an XRP robot
+ */
+public class XRPMotor implements MotorController {
+  private static HashMap<Integer, String> s_simDeviceNameMap = new HashMap<>();
+  private static HashSet<Integer> s_registeredDevices = new HashSet<>();
+
+  private static void checkDeviceAllocation(int deviceNum) {
+    if (!s_simDeviceNameMap.containsKey(deviceNum)) {
+      throw new IllegalArgumentException("Invalid XRPMotor device number. Should be 0-3");
+    }
+
+    if (s_registeredDevices.contains(deviceNum)) {
+      throw new IllegalArgumentException("XRPMotor " + deviceNum + " already allocated");
+    }
+
+    s_registeredDevices.add(deviceNum);
+  }
+
+  static {
+    s_simDeviceNameMap.put(0, "motorL");
+    s_simDeviceNameMap.put(1, "motorR");
+    s_simDeviceNameMap.put(2, "motor3");
+    s_simDeviceNameMap.put(3, "motor4");
+  }
+
+  private final SimDouble m_simSpeed;
+  private final SimBoolean m_simInverted;
+
+  /** XRPMotor. */
+  public XRPMotor(int deviceNum) {
+    checkDeviceAllocation(deviceNum);
+
+    // We want this to appear on the WS messages as type: "XRPMotor", device: <motor name>
+    String simDeviceName = "XRPMotor:" + s_simDeviceNameMap.get(deviceNum);
+    SimDevice xrpMotorSimDevice = SimDevice.create(simDeviceName);
+
+    if (xrpMotorSimDevice != null) {
+      xrpMotorSimDevice.createBoolean("init", Direction.kOutput, true);
+      m_simInverted = xrpMotorSimDevice.createBoolean("inverted", Direction.kInput, false);
+      m_simSpeed = xrpMotorSimDevice.createDouble("speed", Direction.kOutput, 0.0);
+    } else {
+      m_simInverted = null;
+      m_simSpeed = null;
+    }
+  }
+
+  @Override
+  public void set(double speed) {
+    if (m_simSpeed != null) {
+      boolean invert = false;
+      if (m_simInverted != null) {
+        invert = m_simInverted.get();
+      }
+
+      m_simSpeed.set(invert ? -speed : speed);
+    }
+  }
+
+  @Override
+  public double get() {
+    if (m_simSpeed != null) {
+      return m_simSpeed.get();
+    }
+
+    return 0.0;
+  }
+
+  @Override
+  public void setInverted(boolean isInverted) {
+    if (m_simInverted != null) {
+      m_simInverted.set(isInverted);
+    }
+  }
+
+  @Override
+  public boolean getInverted() {
+    if (m_simInverted != null) {
+      return m_simInverted.get();
+    }
+    return false;
+  }
+
+  @Override
+  public void disable() {
+    set(0.0);
+  }
+
+  @Override
+  public void stopMotor() {
+    set(0.0);
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/xrptimed/devices/XRPServo.java

@@ -0,0 +1,123 @@
+// 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.templates.xrptimed.devices;
+
+import edu.wpi.first.hal.SimDevice;
+import edu.wpi.first.hal.SimDevice.Direction;
+import edu.wpi.first.hal.SimDouble;
+import java.util.HashMap;
+import java.util.HashSet;
+
+/**
+ * XRPServo.
+ *
+ * <p>A SimDevice based servo
+ */
+public class XRPServo {
+  private static HashMap<Integer, String> s_simDeviceNameMap = new HashMap<>();
+  private static HashSet<Integer> s_registeredDevices = new HashSet<>();
+
+  private static void checkDeviceAllocation(int deviceNum) {
+    if (!s_simDeviceNameMap.containsKey(deviceNum)) {
+      throw new IllegalArgumentException("Invalid XRPServo device number. Should be 4-5");
+    }
+
+    if (s_registeredDevices.contains(deviceNum)) {
+      throw new IllegalArgumentException("XRPServo " + deviceNum + " already allocated");
+    }
+
+    s_registeredDevices.add(deviceNum);
+  }
+
+  static {
+    s_simDeviceNameMap.put(4, "servo1");
+    s_simDeviceNameMap.put(5, "servo2");
+  }
+
+  private final SimDouble m_simPosition;
+
+  /** XRPServo. */
+  public XRPServo(int deviceNum) {
+    checkDeviceAllocation(deviceNum);
+
+    // We want this to appear on WS as type: "XRPServo", device: <servo name>
+    String simDeviceName = "XRPServo:" + s_simDeviceNameMap.get(deviceNum);
+    SimDevice xrpServoSimDevice = SimDevice.create(simDeviceName);
+
+    if (xrpServoSimDevice != null) {
+      xrpServoSimDevice.createBoolean("init", Direction.kOutput, true);
+      // This should mimic PWM position [0.0, 1.0]
+      m_simPosition = xrpServoSimDevice.createDouble("position", Direction.kOutput, 0.5);
+    } else {
+      m_simPosition = null;
+    }
+  }
+
+  /**
+   * Set the servo angle.
+   *
+   * @param angle Desired angle in degrees
+   */
+  public void setAngle(double angle) {
+    if (angle < 0.0) {
+      angle = 0.0;
+    }
+
+    if (angle > 180.0) {
+      angle = 180.0;
+    }
+
+    double pos = angle / 180.0;
+
+    if (m_simPosition != null) {
+      m_simPosition.set(pos);
+    }
+  }
+
+  /**
+   * Get the servo angle.
+   *
+   * @return Current servo angle
+   */
+  public double getAngle() {
+    if (m_simPosition != null) {
+      return m_simPosition.get() * 180.0;
+    }
+
+    return 90.0;
+  }
+
+  /**
+   * Set the servo position.
+   *
+   * @param pos Desired position (Between 0.0 and 1.0)
+   */
+  public void setPosition(double pos) {
+    if (pos < 0.0) {
+      pos = 0.0;
+    }
+
+    if (pos > 1.0) {
+      pos = 1.0;
+    }
+
+    if (m_simPosition != null) {
+      m_simPosition.set(pos);
+    }
+  }
+
+  /**
+   * Get the servo position.
+   *
+   * @return Current servo position
+   */
+  public double getPosition() {
+    if (m_simPosition != null) {
+      return m_simPosition.get();
+    }
+
+    return 0.5;
+  }
+}