Simple Elevator Example

Below is an example of a simple elevator subsystem. Forward motion pulls the elevator up.

public class Elevator extends Subsystem {
  // CAN ID = 2
  public static int canId = 2;
  
  // How many inches the elavator tranvels
  // per radian that the motor turns
  public static double inchesPerRad = 2.5; 
  // where theta is rotation in radians and x is travel in inches:
  // - theta(x) = x / inchesPerRad
  // - x(theta) = theta * inchesPerRad
  
  // Thrify Nova driving NEO
  private ThriftyNova pivot;
  
  // From NEO internal encoder units, to radians
  private Conversion converter; 
  
  // Enum which contains setpoint data
  enum SetPoint {
    BOTTOM(0), LOW(18), MID(32), HIGH(42);
    final double pos;
    private SetPoint(double pos) { this.pos = pos; }
  }
  public SetPoint setPoint = SetPoint.BOTTOM;
  
  // Storing the real position 
  private double realPosition = 0;
  
  private Elevator() {
  
    converter = new Conversion(PositionUnit.RADIANS, EncoderType.INTERNAL);
    
    pivot = new ThriftyNova(canId) // create with CAN ID
      .setBrakeMode(true) // brake mode
      .setInverted(false) // not inverted 
      .setRampUp(0.25)    // 1/4 second ramp up
      .setRampDown(0.05)  // tiny ramp dowm
      
      .setMaxOutput(1, 0.25)  // full power for forward because the 
                              // system is fighting against gravity
                              // limits power on reverse because the 
                              // system is falling with gravity
                              
      .setSoftLimits(0, 7 * Math.PI) // constrain the motor [0, 4pi]
      .enableSoftLimits(true)        // enable the soft limits
      
      .setMaxCurrent(CurrentType.SUPPLY, 50) // set a 50amp current limit
                                             // on supply side
      
      .useEncoderType(EncoderType.INTERNAL) // use internal NEO encoder
      .usePIDSlot(PIDSlot.SLOT0)            // use the first PID slot
      
      // Configure the first PID slot
      .pid0.setP(0.5) 
      .pid0.setI(0)
      .pid0.setD(0)
      .pid0.setFF(1.2);
      
    // Iterate through errors and check them
    for (var err : pivot.getErrors()) {
    // The user can handle the errors
      System.err.println("Error", err.toString());
    }
    // Clear errors here
    pivot.clearErrors();
  }
  
  // This method is called periodically
  public void update() {
    // Set the position based on the setpoint
    pivot.setPosition(convertor.toMotor(setPoint.pos / inchesPerRad));
    
    // Get the real position from the encoder
   realPosition = convertor.fromMotor(pivot.getPosition()) * inchesPerRad;
    
    // Log the real position and the current draw
    SmartDashboard.putNumber("Elevator Position", realPosition); 
    SmartDashboard.putNumber("Elevator Current", pivot.getCurrent()); 
  }
  
  // Is the elevator at the correct setpoint
  public boolean atSetpoint() {
    // Return true if the absolute distance is under the 1/4 inch tolerance
    return Math.abs(realPosition - setPoint.pos) < 0.25; 
  } 
  
  // Begin singleton boilerplate
  private static volatile Elevator instance;
  public static synchronized Elevator getInstance() { 
    return instance == null ? instance = new Elevator() : instance;
  }
  // End singleton boilerplate
}