Simple Arm Example

Below is an example of a simple arm subsystem. Visualize the arm from the side as overlaying unit circle, where zeroed arm lies on the +x axis.

public class Arm extends Subsystem {
  // CAN ID = 4
  public static int canId = 4;
  
  // How many inches the elavator tranvels
  // per radian that the motor turns
  public static double gearRatio = 17.06; 
  
  // Thrify Nova driving NEO
  private ThriftyNova pivot;
  
  // From NEO internal encoder units, to radians
  private Conversion converter; 
  
  // Enum which contains setpoint data from arm
  // assume zero position is straight out in front and up is +rotation
  // visualize as unit circle where zeroed arm lies on the +x axis
  enum SetPoint {
    TOP(67),
    MIDDLE(27),
    OTHER_SIDE(254),
    BOTTOM(-33);
    final Rotation2d theta;
    private SetPoint(double deg) { this(Rotation2d.fromDegrees(deg)); }
    private SetPoint(Rotation2d theta) { this.theta = theta; }

  }
  public SetPoint setPoint = SetPoint.BOTTOM;

  // Some constraints to not let the arm rotate past
  private static final Rotation2d LOW_CONSTRAINT = Rotation2d.fromDegrees(-33);
  private static final Rotation2d HIGH_CONSTRAINT = Rotation2d.fromDegrees(330);
  
  // Storing the real angle of the arm 
  private double realTheta = 0;
  
  private Arm() {
  
    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
                              
      // constrain the motor
      .setSoftLimits(
        converter.toMotor(LOW_CONSTRAINT.getRadians * gearRatio),
        converter.toMotor(HIGH_CONSTRAINT.getRadians * gearRatio)
      ) 
      .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
    pivot.pid0.setP(0.5)
        .setI(0)
        .setD(0)
        .setFF(1.18);

      
    // 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(converter.toMotor(setPoint.pos * gearRatio));
    
    // Get the real position from the encoder
    realTheta = converter.fromMotor(pivot.getPosition()) / gearRatio;
    
    // Log the real position and the current draw
    SmartDashboard.putNumber("Arm Position", realTheta); 
    SmartDashboard.putNumber("Arm Current", pivot.getSupplyCurrent()); 
  }
  
  // Is the arm at the correct setpoint
  public boolean atSetpoint() {
    // Return true if the absolute distance is under the 2 deg tolerance
    return Math.abs(realTheta - setPoint.theta.getDegrees()) < 2; 
  } 
  
  // Begin singleton boilerplate
  private static volatile Arm instance;
  public static synchronized Arm getInstance() { 
    return instance == null ? instance = new Arm() : instance;
  }
  // End singleton boilerplate
}