/**
* ***************************
*
* <p>Completes everything required for PID to work: - Sets encoder/potentiometer type - Sets the
* P, I, D, and F terms - configures min and max voltages
*
* <p>***************************
*/
private void setupPID() {
leftMotor1.setFeedbackDevice(FeedbackDevice.QuadEncoder);
rightMotor1.setFeedbackDevice(FeedbackDevice.QuadEncoder);
leftMotor1.configNominalOutputVoltage(+0f, -0f);
rightMotor1.configNominalOutputVoltage(+0f, -0f);
leftMotor1.configPeakOutputVoltage(+12.0f, -12.0f);
rightMotor1.configPeakOutputVoltage(+12.0f, -12.0f);
// leftMotor1.
setPID(0.62, 0.006, 0.0, 0.36341);
}
private static CANTalon initCANTalon(
CANTalon talon,
FeedbackDevice device,
boolean reverseSensor,
int codesPerRev,
int acceptableErr,
double rightDriveKp,
double rightDriveKi,
double rightDriveKd) {
talon.reset();
talon.enableZeroSensorPositionOnIndex(true, true);
talon.setPosition(0);
talon.setFeedbackDevice(device);
talon.reverseSensor(reverseSensor);
if (FeedbackDevice.QuadEncoder.equals(device)) talon.configEncoderCodesPerRev(codesPerRev);
else if (FeedbackDevice.AnalogPot.equals(device)) talon.configPotentiometerTurns(codesPerRev);
talon.configNominalOutputVoltage(NOMINAL_FORWARD_VOLTAGE, NOMINAL_REVERSE_VOLTAGE);
talon.configPeakOutputVoltage(PEAK_FORWARD_VOLTAGE, PEAK_REVERSE_VOLTAGE);
talon.setAllowableClosedLoopErr(acceptableErr);
talon.setPID(rightDriveKp, rightDriveKi, rightDriveKd);
return talon;
}
public MasterCANTalon(int port) {
talon = new CANTalon(port);
talon.setFeedbackDevice(FeedbackDevice.CtreMagEncoder_Relative);
talon.reverseSensor(false);
talon.setProfile(0);
talon.setF(0.1722);
talon.setP(0.1);
talon.setI(0);
talon.setD(0);
talon.configNominalOutputVoltage(+0.0f, -0.0f);
talon.configPeakOutputVoltage(+12.0f, -12.0f);
talon.changeControlMode(TalonControlMode.Speed);
}