public DriveTrainSubsystem() {
RIGHT_MOTOR1 = new CANTalon(RobotMap.DRIVERIGHT_MOTOR1);
RIGHT_MOTOR2 = new CANTalon(RobotMap.DRIVERIGHT_MOTOR2);
LEFT_MOTOR1 = new CANTalon(RobotMap.DRIVELEFT_MOTOR1);
LEFT_MOTOR2 = new CANTalon(RobotMap.DRIVELEFT_MOTOR2);
LEFT_MOTOR1.setFeedbackDevice(FeedbackDevice.QuadEncoder);
LEFT_MOTOR1.setPosition(0);
RIGHT_MOTOR1.setFeedbackDevice(FeedbackDevice.QuadEncoder);
RIGHT_MOTOR1.setPosition(0);
LiveWindow.addActuator("Drive", "R1", RIGHT_MOTOR1);
LiveWindow.addActuator("Drive", "R2", RIGHT_MOTOR2);
LiveWindow.addActuator("Drive", "L1", LEFT_MOTOR1);
LiveWindow.addActuator("Drive", "L2", LEFT_MOTOR2);
}
/** This method resets the motor position sensor, typically an encoder. */
public void resetPosition() {
if (feedbackDeviceIsPot) {
zeroPosition = super.getPosition();
} else {
super.setPosition(0.0);
}
} // resetPosition
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;
}
// static initializer
public static void initialize() {
if (!initialized) {
gamepad = new Joystick(GAMEPAD_ID);
initialized = true;
teleopMode = false;
// create and initialize arm motor
frontArmMotor = new CANTalon(FRONT_ARM_MOTOR_ID);
if (frontArmMotor != null) {
System.out.println("Initializing front arm motor (position control)...");
// VERY IMPORTANT - resets talon faults to render them usable again!!
frontArmMotor.clearStickyFaults();
frontArmMotor.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder);
frontArmMotor.changeControlMode(CANTalon.TalonControlMode.PercentVbus);
// set brake mode
frontArmMotor.enableBrakeMode(true);
// initializes encoder to zero
frontArmMotor.setPosition(0);
} else System.out.println("ERROR: Front Arm motor not initialized!");
// create and initialize roller motor
frontArmRollerMotor = new CANTalon(FRONT_ARM_ROLLER_ID);
if (frontArmRollerMotor != null) {
System.out.println("Initializing front arm roller motor (PercentVbus control)...");
frontArmRollerMotor.clearStickyFaults();
// set up roller motor for percent Vbus control mode
frontArmRollerMotor.changeControlMode(CANTalon.TalonControlMode.PercentVbus);
frontArmRollerMotor.enableBrakeMode(false);
// initializes speed of rollers to zero
frontArmRollerMotor.set(0);
} else System.out.println("ERROR: Front Arm roller motor not initialized!");
}
}
/**
* Set the position of the associated encoder
*
* @param position - the desired position
*/
public synchronized void setPosition(double position) {
m_talon.setPosition(position);
reportPosition(position);
}
public void resetEncoder() {
LEFT_MOTOR1.setPosition(0);
RIGHT_MOTOR1.setPosition(0);
}
