[examples] ArmSimulation, ElevatorSimulation: Extract mechanism to class (#5052)

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Constants.java

@@ -0,0 +1,31 @@
+// 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.armsimulation;
+
+import edu.wpi.first.math.util.Units;
+
+public class Constants {
+  public static final int kMotorPort = 0;
+  public static final int kEncoderAChannel = 0;
+  public static final int kEncoderBChannel = 1;
+  public static final int kJoystickPort = 0;
+
+  public static final String kArmPositionKey = "ArmPosition";
+  public static final String kArmPKey = "ArmP";
+
+  // The P gain for the PID controller that drives this arm.
+  public static final double kDefaultArmKp = 50.0;
+  public static final double kDefaultArmSetpointDegrees = 75.0;
+
+  // distance per pulse = (angle per revolution) / (pulses per revolution)
+  //  = (2 * PI rads) / (4096 pulses)
+  public static final double kArmEncoderDistPerPulse = 2.0 * Math.PI / 4096;
+
+  public static final double kArmReduction = 200;
+  public static final double kArmMass = 8.0; // Kilograms
+  public static final double kArmLength = Units.inchesToMeters(30);
+  public static final double kMinAngleRads = Units.degreesToRadians(-75);
+  public static final double kMaxAngleRads = Units.degreesToRadians(255);
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Robot.java

@@ -4,150 +4,48 @@
 
 package edu.wpi.first.wpilibj.examples.armsimulation;
 
-import edu.wpi.first.math.VecBuilder;
-import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.math.system.plant.DCMotor;
-import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.Preferences;
-import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
-import edu.wpi.first.wpilibj.simulation.BatterySim;
-import edu.wpi.first.wpilibj.simulation.EncoderSim;
-import edu.wpi.first.wpilibj.simulation.RoboRioSim;
-import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
-import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
-import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
-import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj.util.Color;
-import edu.wpi.first.wpilibj.util.Color8Bit;
+import edu.wpi.first.wpilibj.examples.armsimulation.subsystems.Arm;
 
 /** This is a sample program to demonstrate the use of arm simulation with existing code. */
 public class Robot extends TimedRobot {
-  private static final int kMotorPort = 0;
-  private static final int kEncoderAChannel = 0;
-  private static final int kEncoderBChannel = 1;
-  private static final int kJoystickPort = 0;
-
-  public static final String kArmPositionKey = "ArmPosition";
-  public static final String kArmPKey = "ArmP";
-
-  // The P gain for the PID controller that drives this arm.
-  private static double kArmKp = 50.0;
-
-  private static double armPositionDeg = 75.0;
-
-  // distance per pulse = (angle per revolution) / (pulses per revolution)
-  //  = (2 * PI rads) / (4096 pulses)
-  private static final double kArmEncoderDistPerPulse = 2.0 * Math.PI / 4096;
-
-  // The arm gearbox represents a gearbox containing two Vex 775pro motors.
-  private final DCMotor m_armGearbox = DCMotor.getVex775Pro(2);
-
-  // Standard classes for controlling our arm
-  private final PIDController m_controller = new PIDController(kArmKp, 0, 0);
-  private final Encoder m_encoder = new Encoder(kEncoderAChannel, kEncoderBChannel);
-  private final PWMSparkMax m_motor = new PWMSparkMax(kMotorPort);
-  private final Joystick m_joystick = new Joystick(kJoystickPort);
-
-  // Simulation classes help us simulate what's going on, including gravity.
-  private static final double m_armReduction = 200;
-  private static final double m_armMass = 8.0; // Kilograms
-  private static final double m_armLength = Units.inchesToMeters(30);
-  // This arm sim represents an arm that can travel from -75 degrees (rotated down front)
-  // to 255 degrees (rotated down in the back).
-  private final SingleJointedArmSim m_armSim =
-      new SingleJointedArmSim(
-          m_armGearbox,
-          m_armReduction,
-          SingleJointedArmSim.estimateMOI(m_armLength, m_armMass),
-          m_armLength,
-          Units.degreesToRadians(-75),
-          Units.degreesToRadians(255),
-          true,
-          VecBuilder.fill(kArmEncoderDistPerPulse) // Add noise with a std-dev of 1 tick
-          );
-  private final EncoderSim m_encoderSim = new EncoderSim(m_encoder);
-
-  // Create a Mechanism2d display of an Arm with a fixed ArmTower and moving Arm.
-  private final Mechanism2d m_mech2d = new Mechanism2d(60, 60);
-  private final MechanismRoot2d m_armPivot = m_mech2d.getRoot("ArmPivot", 30, 30);
-  private final MechanismLigament2d m_armTower =
-      m_armPivot.append(new MechanismLigament2d("ArmTower", 30, -90));
-  private final MechanismLigament2d m_arm =
-      m_armPivot.append(
-          new MechanismLigament2d(
-              "Arm",
-              30,
-              Units.radiansToDegrees(m_armSim.getAngleRads()),
-              6,
-              new Color8Bit(Color.kYellow)));
+  private final Arm m_arm = new Arm();
+  private final Joystick m_joystick = new Joystick(Constants.kJoystickPort);
 
   @Override
-  public void robotInit() {
-    m_encoder.setDistancePerPulse(kArmEncoderDistPerPulse);
-
-    // Put Mechanism 2d to SmartDashboard
-    SmartDashboard.putData("Arm Sim", m_mech2d);
-    m_armTower.setColor(new Color8Bit(Color.kBlue));
-
-    // Set the Arm position setpoint and P constant to Preferences if the keys don't already exist
-    if (!Preferences.containsKey(kArmPositionKey)) {
-      Preferences.setDouble(kArmPositionKey, armPositionDeg);
-    }
-    if (!Preferences.containsKey(kArmPKey)) {
-      Preferences.setDouble(kArmPKey, kArmKp);
-    }
-  }
+  public void robotInit() {}
 
   @Override
   public void simulationPeriodic() {
-    // In this method, we update our simulation of what our arm is doing
-    // First, we set our "inputs" (voltages)
-    m_armSim.setInput(m_motor.get() * RobotController.getBatteryVoltage());
-
-    // Next, we update it. The standard loop time is 20ms.
-    m_armSim.update(0.020);
-
-    // Finally, we set our simulated encoder's readings and simulated battery voltage
-    m_encoderSim.setDistance(m_armSim.getAngleRads());
-    // SimBattery estimates loaded battery voltages
-    RoboRioSim.setVInVoltage(
-        BatterySim.calculateDefaultBatteryLoadedVoltage(m_armSim.getCurrentDrawAmps()));
-
-    // Update the Mechanism Arm angle based on the simulated arm angle
-    m_arm.setAngle(Units.radiansToDegrees(m_armSim.getAngleRads()));
+    m_arm.simulationPeriodic();
   }
 
   @Override
   public void teleopInit() {
-    // Read Preferences for Arm setpoint and kP on entering Teleop
-    armPositionDeg = Preferences.getDouble(kArmPositionKey, armPositionDeg);
-    if (kArmKp != Preferences.getDouble(kArmPKey, kArmKp)) {
-      kArmKp = Preferences.getDouble(kArmPKey, kArmKp);
-      m_controller.setP(kArmKp);
-    }
+    m_arm.loadPreferences();
   }
 
   @Override
   public void teleopPeriodic() {
     if (m_joystick.getTrigger()) {
-      // Here, we run PID control like normal, with a constant setpoint of 75 degrees.
-      var pidOutput =
-          m_controller.calculate(m_encoder.getDistance(), Units.degreesToRadians(armPositionDeg));
-      m_motor.setVoltage(pidOutput);
+      // Here, we run PID control like normal.
+      m_arm.reachSetpoint();
     } else {
       // Otherwise, we disable the motor.
-      m_motor.set(0.0);
+      m_arm.stop();
     }
   }
 
+  @Override
+  public void close() {
+    m_arm.close();
+    super.close();
+  }
+
   @Override
   public void disabledInit() {
     // This just makes sure that our simulation code knows that the motor's off.
-    m_motor.set(0.0);
+    m_arm.stop();
   }
 }

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/subsystems/Arm.java

@@ -0,0 +1,134 @@
+// 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.armsimulation.subsystems;
+
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.Preferences;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.examples.armsimulation.Constants;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+import edu.wpi.first.wpilibj.simulation.BatterySim;
+import edu.wpi.first.wpilibj.simulation.EncoderSim;
+import edu.wpi.first.wpilibj.simulation.RoboRioSim;
+import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
+import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj.util.Color;
+import edu.wpi.first.wpilibj.util.Color8Bit;
+
+public class Arm implements AutoCloseable {
+  // The P gain for the PID controller that drives this arm.
+  private double m_armKp = Constants.kDefaultArmKp;
+  private double m_armSetpointDegrees = Constants.kDefaultArmSetpointDegrees;
+
+  // The arm gearbox represents a gearbox containing two Vex 775pro motors.
+  private final DCMotor m_armGearbox = DCMotor.getVex775Pro(2);
+
+  // Standard classes for controlling our arm
+  private final PIDController m_controller = new PIDController(m_armKp, 0, 0);
+  private final Encoder m_encoder =
+      new Encoder(Constants.kEncoderAChannel, Constants.kEncoderBChannel);
+  private final PWMSparkMax m_motor = new PWMSparkMax(Constants.kMotorPort);
+
+  // Simulation classes help us simulate what's going on, including gravity.
+  // This arm sim represents an arm that can travel from -75 degrees (rotated down front)
+  // to 255 degrees (rotated down in the back).
+  private final SingleJointedArmSim m_armSim =
+      new SingleJointedArmSim(
+          m_armGearbox,
+          Constants.kArmReduction,
+          SingleJointedArmSim.estimateMOI(Constants.kArmLength, Constants.kArmMass),
+          Constants.kArmLength,
+          Constants.kMinAngleRads,
+          Constants.kMaxAngleRads,
+          true,
+          VecBuilder.fill(Constants.kArmEncoderDistPerPulse) // Add noise with a std-dev of 1 tick
+          );
+  private final EncoderSim m_encoderSim = new EncoderSim(m_encoder);
+
+  // Create a Mechanism2d display of an Arm with a fixed ArmTower and moving Arm.
+  private final Mechanism2d m_mech2d = new Mechanism2d(60, 60);
+  private final MechanismRoot2d m_armPivot = m_mech2d.getRoot("ArmPivot", 30, 30);
+  private final MechanismLigament2d m_armTower =
+      m_armPivot.append(new MechanismLigament2d("ArmTower", 30, -90));
+  private final MechanismLigament2d m_arm =
+      m_armPivot.append(
+          new MechanismLigament2d(
+              "Arm",
+              30,
+              Units.radiansToDegrees(m_armSim.getAngleRads()),
+              6,
+              new Color8Bit(Color.kYellow)));
+
+  /** Subsystem constructor. */
+  public Arm() {
+    m_encoder.setDistancePerPulse(Constants.kArmEncoderDistPerPulse);
+
+    // Put Mechanism 2d to SmartDashboard
+    SmartDashboard.putData("Arm Sim", m_mech2d);
+    m_armTower.setColor(new Color8Bit(Color.kBlue));
+
+    // Set the Arm position setpoint and P constant to Preferences if the keys don't already exist
+    Preferences.initDouble(Constants.kArmPositionKey, m_armSetpointDegrees);
+    Preferences.initDouble(Constants.kArmPKey, m_armKp);
+  }
+
+  /** Update the simulation model. */
+  public void simulationPeriodic() {
+    // In this method, we update our simulation of what our arm is doing
+    // First, we set our "inputs" (voltages)
+    m_armSim.setInput(m_motor.get() * RobotController.getBatteryVoltage());
+
+    // Next, we update it. The standard loop time is 20ms.
+    m_armSim.update(0.020);
+
+    // Finally, we set our simulated encoder's readings and simulated battery voltage
+    m_encoderSim.setDistance(m_armSim.getAngleRads());
+    // SimBattery estimates loaded battery voltages
+    RoboRioSim.setVInVoltage(
+        BatterySim.calculateDefaultBatteryLoadedVoltage(m_armSim.getCurrentDrawAmps()));
+
+    // Update the Mechanism Arm angle based on the simulated arm angle
+    m_arm.setAngle(Units.radiansToDegrees(m_armSim.getAngleRads()));
+  }
+
+  /** Load setpoint and kP from preferences. */
+  public void loadPreferences() {
+    // Read Preferences for Arm setpoint and kP on entering Teleop
+    m_armSetpointDegrees = Preferences.getDouble(Constants.kArmPositionKey, m_armSetpointDegrees);
+    if (m_armKp != Preferences.getDouble(Constants.kArmPKey, m_armKp)) {
+      m_armKp = Preferences.getDouble(Constants.kArmPKey, m_armKp);
+      m_controller.setP(m_armKp);
+    }
+  }
+
+  /** Run the control loop to reach and maintain the setpoint from the preferences. */
+  public void reachSetpoint() {
+    var pidOutput =
+        m_controller.calculate(
+            m_encoder.getDistance(), Units.degreesToRadians(m_armSetpointDegrees));
+    m_motor.setVoltage(pidOutput);
+  }
+
+  public void stop() {
+    m_motor.set(0.0);
+  }
+
+  @Override
+  public void close() {
+    m_motor.close();
+    m_encoder.close();
+    m_mech2d.close();
+    m_armPivot.close();
+    m_controller.close();
+    m_arm.close();
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Constants.java

@@ -0,0 +1,37 @@
+// 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.elevatorsimulation;
+
+import edu.wpi.first.math.util.Units;
+
+public class Constants {
+  public static final int kMotorPort = 0;
+  public static final int kEncoderAChannel = 0;
+  public static final int kEncoderBChannel = 1;
+  public static final int kJoystickPort = 0;
+
+  public static final double kElevatorKp = 5;
+  public static final double kElevatorKi = 0;
+  public static final double kElevatorKd = 0;
+
+  public static final double kElevatorkS = 0.0; // volts (V)
+  public static final double kElevatorkG = 0.762; // volts (V)
+  public static final double kElevatorkV = 0.762; // volt per velocity (V/(m/s))
+  public static final double kElevatorkA = 0.0; // volt per acceleration (V/(m/s²))
+
+  public static final double kElevatorGearing = 10.0;
+  public static final double kElevatorDrumRadius = Units.inchesToMeters(2.0);
+  public static final double kCarriageMass = 4.0; // kg
+
+  public static final double kSetpointMeters = 0.75;
+  // Encoder is reset to measure 0 at the bottom, so minimum height is 0.
+  public static final double kMinElevatorHeightMeters = 0.0;
+  public static final double kMaxElevatorHeightMeters = 1.25;
+
+  // distance per pulse = (distance per revolution) / (pulses per revolution)
+  //  = (Pi * D) / ppr
+  public static final double kElevatorEncoderDistPerPulse =
+      2.0 * Math.PI * kElevatorDrumRadius / 4096;
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Robot.java

@@ -4,137 +4,50 @@
 
 package edu.wpi.first.wpilibj.examples.elevatorsimulation;
 
-import edu.wpi.first.math.VecBuilder;
-import edu.wpi.first.math.controller.ElevatorFeedforward;
-import edu.wpi.first.math.controller.ProfiledPIDController;
-import edu.wpi.first.math.system.plant.DCMotor;
-import edu.wpi.first.math.trajectory.TrapezoidProfile;
-import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
-import edu.wpi.first.wpilibj.simulation.BatterySim;
-import edu.wpi.first.wpilibj.simulation.ElevatorSim;
-import edu.wpi.first.wpilibj.simulation.EncoderSim;
-import edu.wpi.first.wpilibj.simulation.RoboRioSim;
-import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
-import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
-import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj.examples.elevatorsimulation.subsystems.Elevator;
 
 /** This is a sample program to demonstrate the use of elevator simulation. */
 public class Robot extends TimedRobot {
-  private static final int kMotorPort = 0;
-  private static final int kEncoderAChannel = 0;
-  private static final int kEncoderBChannel = 1;
-  private static final int kJoystickPort = 0;
-
-  private static final double kElevatorKp = 5;
-  private static final double kElevatorKi = 0;
-  private static final double kElevatorKd = 0;
-
-  private static final double kElevatorkS = 0.0; // volts (V)
-  private static final double kElevatorkG = 0.762; // volts (V)
-  private static final double kElevatorkV = 0.762; // volt per velocity (V/(m/s))
-  private static final double kElevatorkA = 0.0; // volt per acceleration (V/(m/s²))
-
-  private static final double kElevatorGearing = 10.0;
-  private static final double kElevatorDrumRadius = Units.inchesToMeters(2.0);
-  private static final double kCarriageMass = 4.0; // kg
-
-  private static final double kSetpoint = Units.inchesToMeters(30);
-  private static final double kMinElevatorHeight = Units.inchesToMeters(2);
-  private static final double kMaxElevatorHeight = Units.inchesToMeters(50);
-
-  // distance per pulse = (distance per revolution) / (pulses per revolution)
-  //  = (Pi * D) / ppr
-  private static final double kElevatorEncoderDistPerPulse =
-      2.0 * Math.PI * kElevatorDrumRadius / 4096;
-
-  // This gearbox represents a gearbox containing 4 Vex 775pro motors.
-  private final DCMotor m_elevatorGearbox = DCMotor.getVex775Pro(4);
-
-  // Standard classes for controlling our elevator
-  private final ProfiledPIDController m_controller =
-      new ProfiledPIDController(
-          kElevatorKp, kElevatorKi, kElevatorKd, new TrapezoidProfile.Constraints(2.45, 2.45));
-  ElevatorFeedforward m_feedforward =
-      new ElevatorFeedforward(kElevatorkS, kElevatorkG, kElevatorkV, kElevatorkA);
-  private final Encoder m_encoder = new Encoder(kEncoderAChannel, kEncoderBChannel);
-  private final PWMSparkMax m_motor = new PWMSparkMax(kMotorPort);
-  private final Joystick m_joystick = new Joystick(kJoystickPort);
-
-  // Simulation classes help us simulate what's going on, including gravity.
-  private final ElevatorSim m_elevatorSim =
-      new ElevatorSim(
-          m_elevatorGearbox,
-          kElevatorGearing,
-          kCarriageMass,
-          kElevatorDrumRadius,
-          kMinElevatorHeight,
-          kMaxElevatorHeight,
-          true,
-          VecBuilder.fill(0.01));
-  private final EncoderSim m_encoderSim = new EncoderSim(m_encoder);
-
-  // Create a Mechanism2d visualization of the elevator
-  private final Mechanism2d m_mech2d = new Mechanism2d(20, 50);
-  private final MechanismRoot2d m_mech2dRoot = m_mech2d.getRoot("Elevator Root", 10, 0);
-  private final MechanismLigament2d m_elevatorMech2d =
-      m_mech2dRoot.append(
-          new MechanismLigament2d(
-              "Elevator", Units.metersToInches(m_elevatorSim.getPositionMeters()), 90));
+  private final Joystick m_joystick = new Joystick(Constants.kJoystickPort);
+  private final Elevator m_elevator = new Elevator();
 
   @Override
-  public void robotInit() {
-    m_encoder.setDistancePerPulse(kElevatorEncoderDistPerPulse);
+  public void robotInit() {}
 
-    // Publish Mechanism2d to SmartDashboard
-    // To view the Elevator Sim in the simulator, select Network Tables -> SmartDashboard ->
-    // Elevator Sim
-    SmartDashboard.putData("Elevator Sim", m_mech2d);
+  @Override
+  public void robotPeriodic() {
+    // Update the telemetry, including mechanism visualization, regardless of mode.
+    m_elevator.updateTelemetry();
   }
 
   @Override
   public void simulationPeriodic() {
-    // In this method, we update our simulation of what our elevator is doing
-    // First, we set our "inputs" (voltages)
-    m_elevatorSim.setInput(m_motor.get() * RobotController.getBatteryVoltage());
-
-    // Next, we update it. The standard loop time is 20ms.
-    m_elevatorSim.update(0.020);
-
-    // Finally, we set our simulated encoder's readings and simulated battery voltage
-    m_encoderSim.setDistance(m_elevatorSim.getPositionMeters());
-    // SimBattery estimates loaded battery voltages
-    RoboRioSim.setVInVoltage(
-        BatterySim.calculateDefaultBatteryLoadedVoltage(m_elevatorSim.getCurrentDrawAmps()));
-
-    // Update elevator visualization with simulated position
-    m_elevatorMech2d.setLength(Units.metersToInches(m_elevatorSim.getPositionMeters()));
+    // Update the simulation model.
+    m_elevator.simulationPeriodic();
   }
 
   @Override
   public void teleopPeriodic() {
     if (m_joystick.getTrigger()) {
-      // Here, we set the constant setpoint of 30in.
-      m_controller.setGoal(kSetpoint);
+      // Here, we set the constant setpoint of 0.75 meters.
+      m_elevator.reachGoal(Constants.kSetpointMeters);
     } else {
       // Otherwise, we update the setpoint to 0.
-      m_controller.setGoal(0.0);
+      m_elevator.reachGoal(0.0);
     }
-    // With the setpoint value we run PID control like normal
-    double pidOutput = m_controller.calculate(m_encoder.getDistance());
-    double feedforwardOutput = m_feedforward.calculate(m_controller.getSetpoint().velocity);
-    m_motor.setVoltage(pidOutput + feedforwardOutput);
   }
-  // To view the Elevator in the simulator, select Network Tables -> SmartDashboard -> Elevator Sim
 
   @Override
   public void disabledInit() {
     // This just makes sure that our simulation code knows that the motor's off.
-    m_motor.set(0.0);
+    m_elevator.stop();
+  }
+
+  @Override
+  public void close() {
+    m_elevator.close();
+    super.close();
   }
 }

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/subsystems/Elevator.java

@@ -0,0 +1,125 @@
+// 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.elevatorsimulation.subsystems;
+
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.controller.ElevatorFeedforward;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.examples.elevatorsimulation.Constants;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+import edu.wpi.first.wpilibj.simulation.BatterySim;
+import edu.wpi.first.wpilibj.simulation.ElevatorSim;
+import edu.wpi.first.wpilibj.simulation.EncoderSim;
+import edu.wpi.first.wpilibj.simulation.PWMSim;
+import edu.wpi.first.wpilibj.simulation.RoboRioSim;
+import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+public class Elevator implements AutoCloseable {
+  // This gearbox represents a gearbox containing 4 Vex 775pro motors.
+  private final DCMotor m_elevatorGearbox = DCMotor.getVex775Pro(4);
+
+  // Standard classes for controlling our elevator
+  private final ProfiledPIDController m_controller =
+      new ProfiledPIDController(
+          Constants.kElevatorKp,
+          Constants.kElevatorKi,
+          Constants.kElevatorKd,
+          new TrapezoidProfile.Constraints(2.45, 2.45));
+  ElevatorFeedforward m_feedforward =
+      new ElevatorFeedforward(
+          Constants.kElevatorkS,
+          Constants.kElevatorkG,
+          Constants.kElevatorkV,
+          Constants.kElevatorkA);
+  private final Encoder m_encoder =
+      new Encoder(Constants.kEncoderAChannel, Constants.kEncoderBChannel);
+  private final PWMSparkMax m_motor = new PWMSparkMax(Constants.kMotorPort);
+
+  // Simulation classes help us simulate what's going on, including gravity.
+  private final ElevatorSim m_elevatorSim =
+      new ElevatorSim(
+          m_elevatorGearbox,
+          Constants.kElevatorGearing,
+          Constants.kCarriageMass,
+          Constants.kElevatorDrumRadius,
+          Constants.kMinElevatorHeightMeters,
+          Constants.kMaxElevatorHeightMeters,
+          true,
+          VecBuilder.fill(0.01));
+  private final EncoderSim m_encoderSim = new EncoderSim(m_encoder);
+  private final PWMSim m_motorSim = new PWMSim(m_motor);
+
+  // Create a Mechanism2d visualization of the elevator
+  private final Mechanism2d m_mech2d = new Mechanism2d(20, 50);
+  private final MechanismRoot2d m_mech2dRoot = m_mech2d.getRoot("Elevator Root", 10, 0);
+  private final MechanismLigament2d m_elevatorMech2d =
+      m_mech2dRoot.append(
+          new MechanismLigament2d("Elevator", m_elevatorSim.getPositionMeters(), 90));
+
+  /** Subsystem constructor. */
+  public Elevator() {
+    m_encoder.setDistancePerPulse(Constants.kElevatorEncoderDistPerPulse);
+
+    // Publish Mechanism2d to SmartDashboard
+    // To view the Elevator visualization, select Network Tables -> SmartDashboard -> Elevator Sim
+    SmartDashboard.putData("Elevator Sim", m_mech2d);
+  }
+
+  /** Advance the simulation. */
+  public void simulationPeriodic() {
+    // In this method, we update our simulation of what our elevator is doing
+    // First, we set our "inputs" (voltages)
+    m_elevatorSim.setInput(m_motorSim.getSpeed() * RobotController.getBatteryVoltage());
+
+    // Next, we update it. The standard loop time is 20ms.
+    m_elevatorSim.update(0.020);
+
+    // Finally, we set our simulated encoder's readings and simulated battery voltage
+    m_encoderSim.setDistance(m_elevatorSim.getPositionMeters());
+    // SimBattery estimates loaded battery voltages
+    RoboRioSim.setVInVoltage(
+        BatterySim.calculateDefaultBatteryLoadedVoltage(m_elevatorSim.getCurrentDrawAmps()));
+  }
+
+  /**
+   * Run control loop to reach and maintain goal.
+   *
+   * @param goal the position to maintain
+   */
+  public void reachGoal(double goal) {
+    m_controller.setGoal(goal);
+
+    // With the setpoint value we run PID control like normal
+    double pidOutput = m_controller.calculate(m_encoder.getDistance());
+    double feedforwardOutput = m_feedforward.calculate(m_controller.getSetpoint().velocity);
+    m_motor.setVoltage(pidOutput + feedforwardOutput);
+  }
+
+  /** Stop the control loop and motor output. */
+  public void stop() {
+    m_controller.setGoal(0.0);
+    m_motor.set(0.0);
+  }
+
+  /** Update telemetry, including the mechanism visualization. */
+  public void updateTelemetry() {
+    // Update elevator visualization with position
+    m_elevatorMech2d.setLength(m_encoder.getDistance());
+  }
+
+  @Override
+  public void close() {
+    m_encoder.close();
+    m_motor.close();
+    m_mech2d.close();
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/potentiometerpid/Robot.java

@@ -8,7 +8,6 @@ import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.wpilibj.AnalogPotentiometer;
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.motorcontrol.MotorController;
 import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 
 /**
@@ -18,7 +17,7 @@ import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 public class Robot extends TimedRobot {
   static final int kPotChannel = 1;
   static final int kMotorChannel = 7;
-  static final int kJoystickChannel = 0;
+  static final int kJoystickChannel = 3;
 
   // The elevator can move 1.5 meters from top to bottom
   static final double kFullHeightMeters = 1.5;
@@ -37,7 +36,7 @@ public class Robot extends TimedRobot {
   // Scaling is handled internally
   private final AnalogPotentiometer m_potentiometer =
       new AnalogPotentiometer(kPotChannel, kFullHeightMeters);
-  private final MotorController m_elevatorMotor = new PWMSparkMax(kMotorChannel);
+  private final PWMSparkMax m_elevatorMotor = new PWMSparkMax(kMotorChannel);
   private final Joystick m_joystick = new Joystick(kJoystickChannel);
 
   private int m_index;
@@ -64,7 +63,17 @@ public class Robot extends TimedRobot {
     if (m_joystick.getTriggerPressed()) {
       // index of the elevator setpoint wraps around.
       m_index = (m_index + 1) % kSetpointsMeters.length;
+      System.out.println("m_index = " + m_index);
       m_pidController.setSetpoint(kSetpointsMeters[m_index]);
     }
   }
+
+  @Override
+  public void close() {
+    m_elevatorMotor.close();
+    m_potentiometer.close();
+    m_pidController.close();
+    m_index = 0;
+    super.close();
+  }
 }