示例#1
0
    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)
示例#2
0
    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)
示例#3
0
class Roz:
    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)

    def setupServos(self):
        for i in range(1, 13):
            # self.controller.writeTwoByteRegister(i, AX_PUNCH, 200)
            self.controller.writeOneByteRegister(i, AX_CW_COMPLIANCE_MARGIN, 2)
            self.controller.writeOneByteRegister(i, AX_CCW_COMPLIANCE_MARGIN,
                                                 2)

    def setupHeadPosition(self):
        self.controller.rampServoTo(HEAD_YAW_ID, HEAD_YAW_CENTER)
        self.controller.rampServoTo(TAIL_YAW_ID, TAIL_YAW_CENTER)

    def isButtonPushed(self):
        return self.button.isPressed()

    def readBatteryVoltage(self):
        return self.controller.readOneByteRegister(1, AX_12_VOLTAGE) / 10.0

    def checkBatteryVoltageAndTemperature(self, servoId):
        data = self.controller.readData(servoId, AX_12_VOLTAGE, 2)
        voltage = data[0] / 10
        temperature = data[1]
        self.log(
            "Watchdog: Servo ID %d - Voltage: %3.1f volts, Temperature: %d C" %
            (servoId, voltage, temperature))
        if voltage < MINIMUM_VOLTAGE:
            self.log('Battery too low: %3.1f volts - shutting down' % voltage)
            self.mainStateMachine.transitionTo(self.shutdownState)
        if temperature > MAXIMUM_TEMPERATURE:
            self.log('Servo %d temperature too high: %d degrees C' %
                     (servoId, temperature))
            self.mainStateMachine.transitionTo(self.shutdownState)

    def mapObstacleSpace(self):
        # do a scan, return the angle (0-center-relative) to the largest chunk of empty space
        # if no empty space is found, return None
        self.controller.rampServoTo(HEAD_YAW_ID, HEAD_YAW_MIN)
        pyb.delay(250)  # pause for the movement to settle
        values = []
        for position in range(HEAD_YAW_MIN * 100, HEAD_YAW_MAX * 100,
                              int(AX_5_DEGREE_INCREMENT * 100)):
            start = pyb.millis()
            end = start + 40
            i_pos = int(position / 100)
            self.controller.setPosition(HEAD_YAW_ID, i_pos)
            values.append(self.frontRangeFinder.getDistance())
            pyb.delay(
                max(0, end -
                    pyb.millis()))  # we want each loop iteration to take 40ms

        self.controller.rampServoTo(HEAD_YAW_ID, HEAD_YAW_CENTER)
        groups = []
        lastIndex = None
        startGroup = 0
        for (index, value) in enumerate(values):
            if value == 50:
                if lastIndex == index - 1:
                    startGroup = lastIndex
                if lastIndex is None:
                    lastIndex = index
            else:
                if lastIndex is not None:
                    groups.append((startGroup, index - 1))
                lastIndex = None
        if values[-1] == 50:
            groups.append((startGroup, len(values) - 1))

        if len(groups) == 0:
            return None

        maxLength = 0
        maxGroup = None
        for pairs in groups:
            groupLength = pairs[1] - pairs[0] + 1
            if groupLength > maxLength:
                maxGroup = pairs
                maxLength = groupLength

        center = (maxGroup[0] + maxGroup[1]) // 2
        centerPosition = arduino_map(center, 0, len(values), 306, 717)
        centerAngle = arduino_map(centerPosition, 0, 1023, 0, 300)
        return centerAngle

    def log(self, logString):
        self.logger.log(logString)

    def readSensors(self):
        self.frontRangeDistance = self.frontRangeFinder.getDistance()
        self.leftRangeDistance = self.leftRangeFinder.getDistance()
        self.rightRangeDistance = self.rightRangeFinder.getDistance()

    def update(self):
        self.readSensors()
        self.mainStateMachine.update()
        self.ikEngine.handleIK()
        self.watchdogStateMachine.update()
        self.heartbeat.update()

    # =====================================
    #
    #       Waiting for Button State
    #

    def enterWaitingForButtonState(self):
        self.log('Entering WaitingForButtonState')
        self.ikEngine.travelX = 0
        self.ikEngine.travelRotZ = 0.0

    def handleWaitingForButtonState(self):
        if self.isButtonPushed():
            self.mainStateMachine.transitionTo(self.waitingForNoButtonState)

    # =====================================
    #
    #       Waiting for No Button State
    #

    def enterWaitingForNoButtonState(self):
        self.log('Entering WaitingForNoButtonState')

    def handleWaitingForNoButtonState(self):
        if not self.isButtonPushed():
            if self.initialRun == "motionDemo":
                self.mainStateMachine.transitionTo(self.motionDemoState)
            else:
                self.mainStateMachine.transitionTo(self.walkingState)

    # =====================================
    #
    #       Walking State
    #

    def enterWalkingState(self):
        self.log('Entering walking state')
        self.ikEngine.travelX = START_FORWARD_SPEED
        self.ikEngine.travelRotZ = FORWARD_ROT_Z
        self.anglingFlag = None

    def handleWalkingState(self):
        if self.ikEngine.travelX < MAX_FORWARD_SPEED:
            self.ikEngine.travelX += 2
        if self.frontRangeDistance < FRONT_SENSOR_OBSTACLE:
            self.anglingFlag = None
            self.mainStateMachine.transitionTo(self.obstacleAvoidanceState)
        elif self.rightRangeDistance < SIDE_SENSOR_OBSTACLE:
            if self.anglingFlag != "left":
                self.log("Obstacle on right side, angling left")
            self.anglingFlag = "left"
            self.ikEngine.travelRotZ = SIDE_OBSTACLE_TURN_SPEED
        elif self.leftRangeDistance < SIDE_SENSOR_OBSTACLE:
            if self.anglingFlag != "right":
                self.log("Obstacle on left side, angling right")
            self.anglingFlag = "right"
            self.ikEngine.travelRotZ = -SIDE_OBSTACLE_TURN_SPEED
        else:
            self.ikEngine.travelRotZ = FORWARD_ROT_Z
            self.anglingFlag = None
        if self.isButtonPushed():
            self.mainStateMachine.transitionTo(self.shutdownState)

    # =====================================
    #
    #       Obstacle Avoidance State
    #

    def enterObstacleAvoidanceState(self):
        self.log('Entering ObstacleAvoidanceState')
        self.ikEngine.travelX = 0
        self.turnTimeoutTime = pyb.millis() + FRONT_OBSTACLE_TURN_TIMEOUT
        if (self.leftRangeDistance < SIDE_SENSOR_CLEAR_OBSTACLE) & (
                self.rightRangeDistance < SIDE_SENSOR_CLEAR_OBSTACLE):
            # stuff on both sides, pick a direction at random to turn
            if pyb.rng() & 1 == 0:
                self.log('Obstacles on both sides, turning right')
                self.ikEngine.travelRotZ = -FRONT_OBSTACLE_TURN_SPEED
            else:
                self.log('Obstacles on both sides, turning left')
                self.ikEngine.travelRotZ = FRONT_OBSTACLE_TURN_SPEED
        elif self.leftRangeDistance < SIDE_SENSOR_CLEAR_OBSTACLE:
            self.log('Obstacle on left side, turning right')
            self.ikEngine.travelRotZ = -FRONT_OBSTACLE_TURN_SPEED
        elif self.rightRangeDistance < SIDE_SENSOR_CLEAR_OBSTACLE:
            self.log('Obstacle on right side, turning left')
            self.ikEngine.travelRotZ = FRONT_OBSTACLE_TURN_SPEED
        else:  # nothing on either side, so pick a side at random
            if pyb.rng() & 1 == 0:
                self.log('Only front obstacle, turning right')
                self.ikEngine.travelRotZ = -FRONT_OBSTACLE_TURN_SPEED
            else:
                self.log('Only front obstacle, turning left')
                self.ikEngine.travelRotZ = FRONT_OBSTACLE_TURN_SPEED

    def handleObstacleAvoidanceState(self):
        if self.frontRangeDistance >= FRONT_SENSOR_OBSTACLE:
            self.mainStateMachine.transitionTo(
                self.obstacleAvoidanceContinueState)
        if pyb.millis() > self.turnTimeoutTime:
            self.log("Obstacle turn timeout, do scan")
            self.mainStateMachine.transitionTo(self.obstacleAvoidanceScanState)
        if self.isButtonPushed():
            self.mainStateMachine.transitionTo(self.shutdownState)

    # =====================================
    #
    #       Obstacle Avoidance Scan State
    #

    def enterObstacleAvoidanceScanState(self):
        # this function shuts down the FSM for a second or two
        self.log('Entering ObstacleAvoidanceScanState')
        self.ikEngine.travelX = 0
        self.ikEngine.travelRotZ = 0
        self.ikEngine.setupForWalk()
        self.ikEngine.bodyPosX = -50  # move the body forwards so the head clears the legs
        self.ikEngine.setupForWalk()
        self.ikEngine.bodyPosX = 0  # it will move back the next time the IK engine runs
        blue = pyb.LED(BLUE_LED)
        blue.on()
        openAngle = self.mapObstacleSpace()
        blue.off()
        if openAngle is None:
            # we didn't find any open areas, so switch to walking mode which will re-trigger obstacle mode again
            self.log("No openings found from scan")
            self.obstacleScanTurnTime = pyb.millis()
        else:
            # The IK Engine uses radians/s for rotation rate, so figure out the delta in radians and thus given a fixed
            # rotation rate figure out how long we need to turn in order to end up pointing in that direction
            openAngleRadians = math.radians(openAngle)
            self.obstacleScanTurnTime = pyb.millis() + int(
                (abs(openAngleRadians) / FRONT_OBSTACLE_TURN_SPEED) * 1000)
            if openAngle > 0:
                self.log("Found opening at angle %d - turning left" %
                         openAngle)
                self.ikEngine.travelRotZ = FRONT_OBSTACLE_TURN_SPEED
            else:
                self.log("Found opening at angle %d - turning right" %
                         openAngle)
                self.ikEngine.travelRotZ = -FRONT_OBSTACLE_TURN_SPEED

    def handleObstacleAvoidanceScanState(self):
        if pyb.millis() >= self.obstacleScanTurnTime:
            self.log("Obstacle scan turn done, back to walking")
            self.mainStateMachine.transitionTo(self.walkingState)
        if self.isButtonPushed():
            self.mainStateMachine.transitionTo(self.shutdownState)

    # =====================================
    #
    #       Obstacle Avoidance Continue State
    #

    def enterObstacleAvoidanceContinueState(self):
        self.log('Entering ObstacleAvoidanceContinueState')
        self.turnTimeoutTime = pyb.millis(
        ) + FRONT_OBSTACLE_TURN_CONTINUE_TIMEOUT

    def handleObstacleAvoidanceContinueState(self):
        if pyb.millis() > self.turnTimeoutTime:
            self.log("Done turning, back to walking")
            self.mainStateMachine.transitionTo(self.walkingState)
        if self.isButtonPushed():
            self.mainStateMachine.transitionTo(self.shutdownState)

    # =====================================
    #
    #       Motion Demo State
    #

    def enterMotionDemoState(self):
        self.log('Entering MotionDemoState')

    def handleMotionDemoState(self):
        if self.watchdogStateMachine.getCurrentStateMillis() > 1000:
            self.mainStateMachine.transitionTo(self.motionDemoXYState)
        if self.isButtonPushed():
            self.mainStateMachine.transitionTo(self.shutdownState)

    # =====================================
    #
    #       Motion Demo XY State
    #

    def enterMotionDemoXYState(self):
        self.log('Entering MotionDemoXYState')
        self.motionDemoAngle = 0  # use a circular motion in X & Y
        self.motionDemoCycleCount = 0
        self.ikEngine.setTransitionTime(100)

    def handleMotionDemoXYState(self):
        if not self.controller.interpolating:
            self.log("handleMotionDemoXYState")
            radianAngle = math.radians(self.motionDemoAngle)
            x = math.cos(radianAngle)
            y = math.sin(radianAngle)
            self.ikEngine.bodyPosX = 50 * x
            self.ikEngine.bodyPosY = 50 * y
            self.motionDemoAngle += 6
            if self.motionDemoAngle >= 360:
                self.motionDemoAngle = 0
                self.motionDemoCycleCount += 1
        if self.motionDemoCycleCount >= 2:
            self.mainStateMachine.transitionTo(self.shutdownState)
            # self.mainStateMachine.transitionTo(self.motionDemoRollState)
        if self.isButtonPushed():
            self.mainStateMachine.transitionTo(self.shutdownState)

    # =====================================
    #
    #       Shutdown State
    #

    def enterShutdownState(self):
        self.ikEngine.travelX = 0
        self.ikEngine.travelRotZ = 0
        self.heartbeat.shutdown()
        self.shutdown = True

    # =====================================
    #
    #       Watchdog State Machine
    #

    def handleWatchdogState(self):
        # round robin the servos when checking voltage and temperature
        # assumes the leg servo ids are 1-LEG_SERVO_COUNT
        # note that checkBatteryVoltageAndTemperature() both checks and does a shutdown if required
        self.checkBatteryVoltageAndTemperature(self.watchdogServoId)
        self.watchdogServoId += 1
        if self.watchdogServoId > LEG_SERVO_COUNT:
            self.watchdogServoId = 1
        self.watchdogStateMachine.transitionTo(self.watchdogWaitState)

    def handleWatchdogWaitState(self):
        elapsedTime = self.watchdogStateMachine.getCurrentStateMillis()
        if elapsedTime > WATCHDOG_TIME_INTERVAL:
            self.watchdogStateMachine.transitionTo(self.watchdogState)
示例#4
0
class Roz:
    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)

    def setupServos(self):
        for i in range(1, 13):
            # self.controller.writeTwoByteRegister(i, AX_PUNCH, 200)
            self.controller.writeOneByteRegister(i, AX_CW_COMPLIANCE_MARGIN, 2)
            self.controller.writeOneByteRegister(i, AX_CCW_COMPLIANCE_MARGIN, 2)

    def setupHeadPosition(self):
        self.controller.rampServoTo(HEAD_YAW_ID, HEAD_YAW_CENTER)
        self.controller.rampServoTo(TAIL_YAW_ID, TAIL_YAW_CENTER)

    def isButtonPushed(self):
        return self.button.isPressed()

    def readBatteryVoltage(self):
        return self.controller.readOneByteRegister(1, AX_12_VOLTAGE) / 10.0

    def checkBatteryVoltageAndTemperature(self, servoId):
        data = self.controller.readData(servoId, AX_12_VOLTAGE, 2)
        voltage = data[0] / 10
        temperature = data[1]
        self.log("Watchdog: Servo ID %d - Voltage: %3.1f volts, Temperature: %d C" % (servoId, voltage, temperature))
        if voltage < MINIMUM_VOLTAGE:
            self.log('Battery too low: %3.1f volts - shutting down' % voltage)
            self.mainStateMachine.transitionTo(self.shutdownState)
        if temperature > MAXIMUM_TEMPERATURE:
            self.log('Servo %d temperature too high: %d degrees C' % (servoId, temperature))
            self.mainStateMachine.transitionTo(self.shutdownState)

    def mapObstacleSpace(self):
        # do a scan, return the angle (0-center-relative) to the largest chunk of empty space
        # if no empty space is found, return None
        self.controller.rampServoTo(HEAD_YAW_ID, HEAD_YAW_MIN)
        pyb.delay(250)  # pause for the movement to settle
        values = []
        for position in range(HEAD_YAW_MIN * 100, HEAD_YAW_MAX * 100, int(AX_5_DEGREE_INCREMENT * 100)):
            start = pyb.millis()
            end = start + 40
            i_pos = int(position / 100)
            self.controller.setPosition(HEAD_YAW_ID, i_pos)
            values.append(self.frontRangeFinder.getDistance())
            pyb.delay(max(0, end - pyb.millis()))  # we want each loop iteration to take 40ms

        self.controller.rampServoTo(HEAD_YAW_ID, HEAD_YAW_CENTER)
        groups = []
        lastIndex = None
        startGroup = 0
        for (index, value) in enumerate(values):
            if value == 50:
                if lastIndex == index - 1:
                    startGroup = lastIndex
                if lastIndex is None:
                    lastIndex = index
            else:
                if lastIndex is not None:
                    groups.append((startGroup, index - 1))
                lastIndex = None
        if values[-1] == 50:
            groups.append((startGroup, len(values) - 1))

        if len(groups) == 0:
            return None

        maxLength = 0
        maxGroup = None
        for pairs in groups:
            groupLength = pairs[1] - pairs[0] + 1
            if groupLength > maxLength:
                maxGroup = pairs
                maxLength = groupLength

        center = (maxGroup[0] + maxGroup[1]) // 2
        centerPosition = arduino_map(center, 0, len(values), 306, 717)
        centerAngle = arduino_map(centerPosition, 0, 1023, 0, 300)
        return centerAngle

    def log(self, logString):
        self.logger.log(logString)

    def readSensors(self):
        self.frontRangeDistance = self.frontRangeFinder.getDistance()
        self.leftRangeDistance = self.leftRangeFinder.getDistance()
        self.rightRangeDistance = self.rightRangeFinder.getDistance()

    def update(self):
        self.readSensors()
        self.mainStateMachine.update()
        self.ikEngine.handleIK()
        self.watchdogStateMachine.update()
        self.heartbeat.update()

    # =====================================
    #
    #       Waiting for Button State
    #

    def enterWaitingForButtonState(self):
        self.log('Entering WaitingForButtonState')
        self.ikEngine.travelX = 0
        self.ikEngine.travelRotZ = 0.0

    def handleWaitingForButtonState(self):
        if self.isButtonPushed():
            self.mainStateMachine.transitionTo(self.waitingForNoButtonState)

    # =====================================
    #
    #       Waiting for No Button State
    #

    def enterWaitingForNoButtonState(self):
        self.log('Entering WaitingForNoButtonState')

    def handleWaitingForNoButtonState(self):
        if not self.isButtonPushed():
            if self.initialRun == "motionDemo":
                self.mainStateMachine.transitionTo(self.motionDemoState)
            else:
                self.mainStateMachine.transitionTo(self.walkingState)

    # =====================================
    #
    #       Walking State
    #

    def enterWalkingState(self):
        self.log('Entering walking state')
        self.ikEngine.travelX = START_FORWARD_SPEED
        self.ikEngine.travelRotZ = FORWARD_ROT_Z
        self.anglingFlag = None

    def handleWalkingState(self):
        if self.ikEngine.travelX < MAX_FORWARD_SPEED:
            self.ikEngine.travelX += 2
        if self.frontRangeDistance < FRONT_SENSOR_OBSTACLE:
            self.anglingFlag = None
            self.mainStateMachine.transitionTo(self.obstacleAvoidanceState)
        elif self.rightRangeDistance < SIDE_SENSOR_OBSTACLE:
            if self.anglingFlag != "left":
                self.log("Obstacle on right side, angling left")
            self.anglingFlag = "left"
            self.ikEngine.travelRotZ = SIDE_OBSTACLE_TURN_SPEED
        elif self.leftRangeDistance < SIDE_SENSOR_OBSTACLE:
            if self.anglingFlag != "right":
                self.log("Obstacle on left side, angling right")
            self.anglingFlag = "right"
            self.ikEngine.travelRotZ = -SIDE_OBSTACLE_TURN_SPEED
        else:
            self.ikEngine.travelRotZ = FORWARD_ROT_Z
            self.anglingFlag = None
        if self.isButtonPushed():
            self.mainStateMachine.transitionTo(self.shutdownState)

    # =====================================
    #
    #       Obstacle Avoidance State
    #

    def enterObstacleAvoidanceState(self):
        self.log('Entering ObstacleAvoidanceState')
        self.ikEngine.travelX = 0
        self.turnTimeoutTime = pyb.millis() + FRONT_OBSTACLE_TURN_TIMEOUT
        if (self.leftRangeDistance < SIDE_SENSOR_CLEAR_OBSTACLE) & (
            self.rightRangeDistance < SIDE_SENSOR_CLEAR_OBSTACLE):
            # stuff on both sides, pick a direction at random to turn
            if pyb.rng() & 1 == 0:
                self.log('Obstacles on both sides, turning right')
                self.ikEngine.travelRotZ = -FRONT_OBSTACLE_TURN_SPEED
            else:
                self.log('Obstacles on both sides, turning left')
                self.ikEngine.travelRotZ = FRONT_OBSTACLE_TURN_SPEED
        elif self.leftRangeDistance < SIDE_SENSOR_CLEAR_OBSTACLE:
            self.log('Obstacle on left side, turning right')
            self.ikEngine.travelRotZ = -FRONT_OBSTACLE_TURN_SPEED
        elif self.rightRangeDistance < SIDE_SENSOR_CLEAR_OBSTACLE:
            self.log('Obstacle on right side, turning left')
            self.ikEngine.travelRotZ = FRONT_OBSTACLE_TURN_SPEED
        else:  # nothing on either side, so pick a side at random
            if pyb.rng() & 1 == 0:
                self.log('Only front obstacle, turning right')
                self.ikEngine.travelRotZ = -FRONT_OBSTACLE_TURN_SPEED
            else:
                self.log('Only front obstacle, turning left')
                self.ikEngine.travelRotZ = FRONT_OBSTACLE_TURN_SPEED

    def handleObstacleAvoidanceState(self):
        if self.frontRangeDistance >= FRONT_SENSOR_OBSTACLE:
            self.mainStateMachine.transitionTo(self.obstacleAvoidanceContinueState)
        if pyb.millis() > self.turnTimeoutTime:
            self.log("Obstacle turn timeout, do scan")
            self.mainStateMachine.transitionTo(self.obstacleAvoidanceScanState)
        if self.isButtonPushed():
            self.mainStateMachine.transitionTo(self.shutdownState)

    # =====================================
    #
    #       Obstacle Avoidance Scan State
    #

    def enterObstacleAvoidanceScanState(self):
        # this function shuts down the FSM for a second or two
        self.log('Entering ObstacleAvoidanceScanState')
        self.ikEngine.travelX = 0
        self.ikEngine.travelRotZ = 0
        self.ikEngine.setupForWalk()
        self.ikEngine.bodyPosX = -50  # move the body forwards so the head clears the legs
        self.ikEngine.setupForWalk()
        self.ikEngine.bodyPosX = 0  # it will move back the next time the IK engine runs
        blue = pyb.LED(BLUE_LED)
        blue.on()
        openAngle = self.mapObstacleSpace()
        blue.off()
        if openAngle is None:
            # we didn't find any open areas, so switch to walking mode which will re-trigger obstacle mode again
            self.log("No openings found from scan")
            self.obstacleScanTurnTime = pyb.millis()
        else:
            # The IK Engine uses radians/s for rotation rate, so figure out the delta in radians and thus given a fixed
            # rotation rate figure out how long we need to turn in order to end up pointing in that direction
            openAngleRadians = math.radians(openAngle)
            self.obstacleScanTurnTime = pyb.millis() + int((abs(openAngleRadians) / FRONT_OBSTACLE_TURN_SPEED) * 1000)
            if openAngle > 0:
                self.log("Found opening at angle %d - turning left" % openAngle)
                self.ikEngine.travelRotZ = FRONT_OBSTACLE_TURN_SPEED
            else:
                self.log("Found opening at angle %d - turning right" % openAngle)
                self.ikEngine.travelRotZ = -FRONT_OBSTACLE_TURN_SPEED

    def handleObstacleAvoidanceScanState(self):
        if pyb.millis() >= self.obstacleScanTurnTime:
            self.log("Obstacle scan turn done, back to walking")
            self.mainStateMachine.transitionTo(self.walkingState)
        if self.isButtonPushed():
            self.mainStateMachine.transitionTo(self.shutdownState)

    # =====================================
    #
    #       Obstacle Avoidance Continue State
    #

    def enterObstacleAvoidanceContinueState(self):
        self.log('Entering ObstacleAvoidanceContinueState')
        self.turnTimeoutTime = pyb.millis() + FRONT_OBSTACLE_TURN_CONTINUE_TIMEOUT

    def handleObstacleAvoidanceContinueState(self):
        if pyb.millis() > self.turnTimeoutTime:
            self.log("Done turning, back to walking")
            self.mainStateMachine.transitionTo(self.walkingState)
        if self.isButtonPushed():
            self.mainStateMachine.transitionTo(self.shutdownState)

    # =====================================
    #
    #       Motion Demo State
    #

    def enterMotionDemoState(self):
        self.log('Entering MotionDemoState')

    def handleMotionDemoState(self):
        if self.watchdogStateMachine.getCurrentStateMillis() > 1000:
            self.mainStateMachine.transitionTo(self.motionDemoXYState)
        if self.isButtonPushed():
            self.mainStateMachine.transitionTo(self.shutdownState)

    # =====================================
    #
    #       Motion Demo XY State
    #

    def enterMotionDemoXYState(self):
        self.log('Entering MotionDemoXYState')
        self.motionDemoAngle = 0  # use a circular motion in X & Y
        self.motionDemoCycleCount = 0
        self.ikEngine.setTransitionTime(100)

    def handleMotionDemoXYState(self):
        if not self.controller.interpolating:
            self.log("handleMotionDemoXYState")
            radianAngle = math.radians(self.motionDemoAngle)
            x = math.cos(radianAngle)
            y = math.sin(radianAngle)
            self.ikEngine.bodyPosX = 50 * x
            self.ikEngine.bodyPosY = 50 * y
            self.motionDemoAngle += 6
            if self.motionDemoAngle >= 360:
                self.motionDemoAngle = 0
                self.motionDemoCycleCount += 1
        if self.motionDemoCycleCount >= 2:
            self.mainStateMachine.transitionTo(self.shutdownState)
            # self.mainStateMachine.transitionTo(self.motionDemoRollState)
        if self.isButtonPushed():
            self.mainStateMachine.transitionTo(self.shutdownState)

    # =====================================
    #
    #       Shutdown State
    #

    def enterShutdownState(self):
        self.ikEngine.travelX = 0
        self.ikEngine.travelRotZ = 0
        self.heartbeat.shutdown()
        self.shutdown = True

    # =====================================
    #
    #       Watchdog State Machine
    #

    def handleWatchdogState(self):
        # round robin the servos when checking voltage and temperature
        # assumes the leg servo ids are 1-LEG_SERVO_COUNT
        # note that checkBatteryVoltageAndTemperature() both checks and does a shutdown if required
        self.checkBatteryVoltageAndTemperature(self.watchdogServoId)
        self.watchdogServoId += 1
        if self.watchdogServoId > LEG_SERVO_COUNT:
            self.watchdogServoId = 1
        self.watchdogStateMachine.transitionTo(self.watchdogWaitState)

    def handleWatchdogWaitState(self):
        elapsedTime = self.watchdogStateMachine.getCurrentStateMillis()
        if elapsedTime > WATCHDOG_TIME_INTERVAL:
            self.watchdogStateMachine.transitionTo(self.watchdogState)