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;
}
/**
* ************************************
*
* <p>Sets the P, I, D, and F terms for both sides of driveTrain
*
* @param p is the proportional term
* @param i is the integral term
* @param d is the derivative term
* @param f is the feed-forward term
* <p>************************************
*/
public void setPID(double p, double i, double d, double f) {
leftMotor1.setPID(p, i, d, f, 300, RAMP_RATE, 0);
rightMotor1.setPID(p, i, d, f, 300, RAMP_RATE, 0);
}