def __init__(self):
self.button = OneShotButton(BUTTON_PIN, False, 0)
if self.button.isPressed():
self.initialRun = "motionDemo"
print("Running Motion Demo")
else:
self.initialRun = "walking"
print("Running Walking")
self.logger = Logger('logfile.txt')
self.controller = BioloidController()
self.setupServos()
self.ikEngine = IKEngine()
self.ikEngine.setController(self.controller)
self.ikEngine.setLogger(self.logger)
self.setupHeadPosition()
self.ikEngine.setTransitionTime(TRANSITION_TIME)
self.ikEngine.setupForWalk()
self.debug = False
self.frontRangeFinder = RangeFinder(FRONT_RANGE_PIN, RANGE_MAX)
self.leftRangeFinder = RangeFinder(LEFT_RANGE_PIN, RANGE_MAX)
self.rightRangeFinder = RangeFinder(RIGHT_RANGE_PIN, RANGE_MAX)
self.readSensors()
self.shutdown = False
self.watchdogServoId = 1
self.heartbeat = HeartbeatLED(RED_LED)
self.waitingForButtonState = State("waitingForButton",
self.enterWaitingForButtonState,
self.handleWaitingForButtonState,
None)
self.waitingForNoButtonState = State(
"waitingForNoButton", self.enterWaitingForNoButtonState,
self.handleWaitingForNoButtonState, None)
self.walkingState = State("walking", self.enterWalkingState,
self.handleWalkingState, None)
self.obstacleAvoidanceState = State("obstacleAvoidance",
self.enterObstacleAvoidanceState,
self.handleObstacleAvoidanceState,
None)
self.obstacleAvoidanceScanState = State(
"obstacleAvoidanceScan", self.enterObstacleAvoidanceScanState,
self.handleObstacleAvoidanceScanState, None)
self.obstacleAvoidanceContinueState = State(
"obstacleAvoidanceContinue",
self.enterObstacleAvoidanceContinueState,
self.handleObstacleAvoidanceContinueState, None)
self.motionDemoState = State("motionDemo", self.enterMotionDemoState,
self.handleMotionDemoState, None)
self.motionDemoXYState = State("motionXYDemo",
self.enterMotionDemoXYState,
self.handleMotionDemoXYState, None)
self.shutdownState = State("shutdown", self.enterShutdownState, None,
None)
self.mainStateMachine = FiniteStateMachine(self.waitingForButtonState)
self.watchdogState = State("watchdog", None, self.handleWatchdogState,
None)
self.watchdogWaitState = State("watchdog-wait", None,
self.handleWatchdogWaitState, None)
self.watchdogStateMachine = FiniteStateMachine(self.watchdogState)
