public AutoBinSet(Robot robot, double distance) {
this.robot = robot;
this.distance = distance;
driveEvent = new TrcEvent(moduleName + ".driveEvent");
elevatorEvent = new TrcEvent(moduleName + ".elevatorEvent");
sm = new TrcStateMachine(moduleName + ".sm");
sm.start();
} // AutoBinSet
//
// Implements TrcRobot.AutoStrategy.
//
public void autoPeriodic() {
boolean ready = sm.isReady();
TrcDashboard.textPrintf(
1, "%s[%d] = %s", moduleName, sm.getState(), ready ? "Ready" : "NotReady");
if (ready) {
int state = sm.getState();
switch (state) {
case STATE_GOTO_BIN:
//
// lift elevator to grabbing height
//
robot.pidDrive.setTarget(0.0, 30.0, 0.0, false, driveEvent, 2.0);
robot.elevator.setHeight(RobotInfo.ELEVATOR_PICKUP_BIN, elevatorEvent, 1.0);
sm.addEvent(driveEvent);
sm.addEvent(elevatorEvent);
sm.waitForEvents(STATE_PICKUP_BIN, true, 0.0);
break;
case STATE_PICKUP_BIN:
if (robot.lowerGrabber != null) {
robot.lowerGrabber.extend();
}
robot.elevator.setHeight(RobotInfo.ELEVATOR_LIFT_BIN, elevatorEvent, 1.0);
sm.addEvent(elevatorEvent);
sm.waitForEvents(STATE_MOVE_AUTOZONE);
break;
case STATE_MOVE_AUTOZONE:
//
// move forward into the auto zone
//
robot.elevator.setHeight(RobotInfo.ELEVATOR_LIFT_BIN);
robot.pidDrive.setTarget(0.0, distance, 0.0, false, driveEvent, 10.0);
sm.addEvent(driveEvent);
sm.waitForEvents(STATE_DONE);
break;
case STATE_DONE:
default:
//
// stop (in the name of love)
//
sm.stop();
}
}
} // autoPeriodic
