public class LineFollower extends IterativeRobot {
final double FORWARD_SPEED = -0.5;
final long T_WAIT_TIME = 500;
final long NUM_TS = 1;
BaseRobot robot = BaseRobot.getInstance();
PIDController controller =
new PIDController(
10,
0,
0,
robot.lineSensor,
new PIDOutput() {
public void pidWrite(double output) {
if (Double.isNaN(output)) System.out.println("Uh oh...");
robot.drive.setDriveVelocity(new Vector(output, FORWARD_SPEED));
}
});
public void autonomousInit() {
try {
robot.arm.moveTo(PegPosition.RESET);
} catch (CANTimeoutException e1) {
// TODO Auto-generated catch block
e1.printStackTrace();
}
robot.compressor.start();
Timer.delay(3.5);
robot.grabber.tiltDown();
Timer.delay(2);
robot.grabber.grab();
robot.lineSensor.setLinePreference(LinePreference.LEFT);
controller.enable();
Timer.delay(2.5);
robot.grabber.tiltUp();
Timer.delay(2.5);
try {
robot.arm.moveTo(PegPosition.MIDDLE_OFFSET);
} catch (CANTimeoutException e) {
// TODO Auto-generated catch block
e.printStackTrace();
}
}
int tCount = 0;
long lastTtime = -T_WAIT_TIME;
public void autonomousContinuous() {
long currentTime = System.currentTimeMillis();
long timeSinceLastT = currentTime - lastTtime;
// If we're not at a T, continue the loop
if (!robot.lineSensor.isAtT()) return;
System.out.println("At a T");
lastTtime = currentTime;
// If we're seeing the same T, continue the loop
if (timeSinceLastT < T_WAIT_TIME) return;
tCount++;
// If this isn't the last T, continue the loop
if (tCount != NUM_TS) return;
controller.disable();
robot.grabber.release();
// Arm goes down
}
public void disabledInit() {
controller.disable();
}
}
