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");
}