public void init() {
try {
TKOHardware.changeTalonMode(
TKOHardware.getLeftDrive(), CANTalon.ControlMode.Position, p, i, d);
TKOHardware.changeTalonMode(
TKOHardware.getRightDrive(), CANTalon.ControlMode.Position, p, i, d);
TKOHardware.getLeftDrive().reverseOutput(false);
TKOHardware.getRightDrive().reverseOutput(true);
TKOHardware.getLeftDrive().reverseSensor(true);
TKOHardware.getRightDrive().reverseSensor(false);
TKOHardware.getLeftDrive().enableBrakeMode(true);
TKOHardware.getRightDrive().enableBrakeMode(true);
TKOHardware.getLeftDrive().setPosition(0); // resets encoder
TKOHardware.getRightDrive().setPosition(0);
TKOHardware.getLeftDrive().ClearIaccum(); // stops bounce
TKOHardware.getRightDrive().ClearIaccum();
Timer.delay(0.1);
TKOHardware.getLeftDrive().set(TKOHardware.getLeftDrive().getPosition());
TKOHardware.getRightDrive().set(TKOHardware.getRightDrive().getPosition());
TKOHardware.getPiston(0).set(Definitions.SHIFTER_LOW);
TKOHardware.getPiston(2).set(Definitions.WHEELIE_RETRACT);
} catch (TKOException e) {
e.printStackTrace();
System.out.println("Err.... Talons kinda died ");
}
System.out.println("Drive atom initialized");
}
@Override
public void execute() {
System.out.println("Starting execution");
try {
if (distance > 0) {
while (DriverStation.getInstance().isEnabled()
&& TKOHardware.getDriveTalon(0).getSetpoint() < distance) {
TKOHardware.getDriveTalon(0)
.set(TKOHardware.getDriveTalon(0).getSetpoint() + incrementer);
TKOHardware.getDriveTalon(2)
.set(TKOHardware.getDriveTalon(2).getSetpoint() + incrementer);
System.out.println(
"Ncoder Left: "
+ TKOHardware.getDriveTalon(0).getPosition()
+ "\t Ncoder Rgith: "
+ TKOHardware.getDriveTalon(2).getPosition()
+ "\t Left Setpoint: "
+ TKOHardware.getDriveTalon(0).getSetpoint());
TKOLogger.getInstance()
.addMessage(
"Ncoder Left: "
+ TKOHardware.getDriveTalon(0).getPosition()
+ "\t Ncoder Rgith: "
+ TKOHardware.getDriveTalon(2).getPosition()
+ "\t Left Setpoint: "
+ TKOHardware.getDriveTalon(0).getSetpoint());
Timer.delay(0.001);
}
} else {
while (DriverStation.getInstance().isEnabled()
&& TKOHardware.getDriveTalon(0).getSetpoint() > distance) {
TKOHardware.getDriveTalon(0)
.set(TKOHardware.getDriveTalon(0).getSetpoint() - incrementer);
TKOHardware.getDriveTalon(2)
.set(TKOHardware.getDriveTalon(2).getSetpoint() - incrementer);
System.out.println(
"Ncoder Left: "
+ TKOHardware.getDriveTalon(0).getPosition()
+ "\t Ncoder Rgith: "
+ TKOHardware.getDriveTalon(2).getPosition()
+ "\t Left Setpoint: "
+ TKOHardware.getDriveTalon(0).getSetpoint());
TKOLogger.getInstance()
.addMessage(
"Ncoder Left: "
+ TKOHardware.getDriveTalon(0).getPosition()
+ "\t Ncoder Rgith: "
+ TKOHardware.getDriveTalon(2).getPosition()
+ "\t Left Setpoint: "
+ TKOHardware.getDriveTalon(0).getSetpoint());
Timer.delay(0.001);
}
}
TKOHardware.getDriveTalon(0).set(distance);
TKOHardware.getDriveTalon(2).set(distance);
while (Math.abs(TKOHardware.getLeftDrive().getPosition() - distance) > threshold
&& DriverStation.getInstance().isEnabled()) {
// not close enough doe; actually gets stuck here
TKOLogger.getInstance()
.addMessage(
"NOT CLOSE ENOUGH TO TARGET DIST: "
+ (TKOHardware.getLeftDrive().getPosition() - distance));
Timer.delay(0.001);
}
} catch (TKOException e1) {
e1.printStackTrace();
System.out.println("Error at another expected spot, I would assume....");
}
System.out.println("Done executing");
}