public void storeLoaderAngle(String sPosition) {
   float volts = (float) leftAngleCh.getVoltage();
   float voltsR = (float) rightAngleCh.getVoltage();
   m_preferences.putFloat(sPosition, volts);
   m_preferences.putFloat(sPosition + "R", voltsR);
   System.out.println(sPosition + " = " + volts);
 }
  public void goToPosition(String sPosition) {

    m_sPosition = sPosition;
    pidLeft.setSetpoint(m_preferences.getFloat(sPosition, 0));
    pidRight.setSetpoint(m_preferences.getFloat(sPosition + "R", 0));

    System.out.println(sPosition + " Setpoint = " + m_preferences.getFloat(sPosition, 0));
  }
  // Initialize your subsystem here
  public LoaderClimberOld2PID() {
    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=PID
    // super("LoaderClimberOld2PID", 1.5, 0.1, 0.0);
    // setAbsoluteTolerance(0.02);
    // getPIDController().setContinuous(false);

    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=PID
    m_preferences = Preferences.getInstance();
    if (leftJag != null) { // for practice robot

      // Fix for motors going in opposite directios
      PIDOutput rightOutput =
          new PIDOutput() {
            public void pidWrite(double output) {
              rightJag.pidWrite(-1 * output);
            }
          };

      pidLeft = new PIDController(P, I, D, leftAngleCh, leftJag); // PID Output Interface
      pidRight = new PIDController(P, I, D, rightAngleCh, rightOutput); // PID Output Interface*/

      LiveWindow.addActuator("LoaderClimber", "PIDLeft Angle Controller", pidLeft);
      LiveWindow.addActuator("LoaderClimber", "PIDLeft Angle Controller", pidRight);

      pidLeft.setSetpoint(m_preferences.getFloat("Retract", 2.4f));
      pidRight.setSetpoint(m_preferences.getFloat("RetractR", 2.4f));

      pidLeft.setInputRange(0, 4);
      pidRight.setInputRange(0, 4);

      pidLeft.setPercentTolerance(PID_TOL);
      pidRight.setPercentTolerance(PID_TOL);

      pidLeft.enable(); // - Enables the PID controller.
      pidRight.enable(); // - Enables the PID controller.
      // disable();

    }
  }
 //    int errors = 0;
 //    protected void usePIDOutput(double output) {
 //
 //        double outputNeg = output * -1;
 //        double slow = output * .815;
 ////        System.out.println("LoaderClimberOld2PID: usePIDOutput, outputNeg=" + outputNeg + "
 // slow=" + slow + "   errors:" + errors);
 //        try {
 //            leftJag.setX(slow);
 //            rightJag.setX(outputNeg);
 //        } catch (CANTimeoutException ex) {
 ////            ex.printStackTrace();
 //            errors = errors + 1;
 //        }
 //    }
 public void savePreferences() {
   m_preferences.save();
   System.out.println("Preferences saved");
 }