public void MoveState() {
switch (moveState) {
default:
System.out.println("default state.. WTF???");
gather.StopFront();
gather.StopBack();
elevator.Stop();
break;
case 0:
System.out.println("Start Your Engine");
gather.FrontForward();
gather.BackForward();
moveState = 1;
break;
case 1:
System.out.println("Load Ball 1");
if (frontGather.isTriggered() || backGather.isTriggered()) {
moveState = 2;
}
break;
case 2:
System.out.println("Move 1 to point 1");
elevator.MoveUp(.35);
if (elevatorSensors[0].isTriggered()) {
elevator.Stop();
moveState = 12;
}
break;
case 12:
System.out.println("State 12");
if (!frontGather.isTriggered() && !backGather.isTriggered()) {
moveState = 3;
}
break;
case 3:
System.out.println("Load Ball 2, adjust to point 2");
if (frontGather.isTriggered() || backGather.isTriggered()) {
elevator.MoveUp(.35);
moveState = 4;
}
break;
case 4:
System.out.println("State 4");
if (elevatorSensors[1].isTriggered()) {
elevator.Stop();
moveState = 14;
}
break;
case 14:
System.out.println("State 14");
if (!frontGather.isTriggered() && !backGather.isTriggered()) {
moveState = 5;
}
break;
case 5:
System.out.println("State 5");
if (frontGather.isTriggered() || backGather.isTriggered()) {
elevator.MoveUp(.4);
moveState = 15;
}
break;
case 15:
System.out.println("State 15");
if (!frontGather.isTriggered() && !backGather.isTriggered()) {
moveState = 6;
}
break;
case 6:
System.out.println("State 6");
if (elevatorSensors[2].isTriggered()) {
elevator.Stop();
moveState = 7;
}
break;
case 7:
System.out.println("END STATE");
gather.StopFront();
gather.StopBack();
elevator.Stop();
break;
}
}
public void startUp() {
gather.FrontForward();
gather.BackForward();
elevator.MoveUp();
}
