// // 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