Exemple #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)
Exemple #2
0
 def __init__(self, useLogger = False):
     self.id = [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12]
     self.pose = [512, 512, 512, 512, 512, 512, 512, 512, 512, 512, 512, 512]
     self.nextPose = [512, 512, 512, 512, 512, 512, 512, 512, 512, 512, 512, 512]
     self.speed = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
     self.interpolating = False
     self.playing = False
     self.servoCount = 12
     self.lastFrame = pyb.millis()
     self.bus = UART_Bus(1, 1000000, show_packets=False)
     if useLogger:
         self.logger = Logger('sync_log.txt')
     else:
         self.logger = None
Exemple #3
0
 def __init__(self, useLogger=False):
     self.id = [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12]
     self.pose = [
         512, 512, 512, 512, 512, 512, 512, 512, 512, 512, 512, 512
     ]
     self.nextPose = [
         512, 512, 512, 512, 512, 512, 512, 512, 512, 512, 512, 512
     ]
     self.speed = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
     self.interpolating = False
     self.playing = False
     self.servoCount = 12
     self.lastFrame = pyb.millis()
     self.port = UART_Port(1, 1000000)
     self.bus = Bus(self.port, show=Bus.SHOW_PACKETS)
     # Bus.SHOW_NONE
     # Bus.SHOW_COMMANDS
     # Bus.SHOW_PACKETS
     if useLogger:
         self.logger = Logger('sync_log.txt')
     else:
         self.logger = None
Exemple #4
0
 def __init__(self, useLogger = False):
     self.id = [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12]
     self.pose = [512, 512, 512, 512, 512, 512, 512, 512, 512, 512, 512, 512]
     self.nextPose = [512, 512, 512, 512, 512, 512, 512, 512, 512, 512, 512, 512]
     self.speed = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
     self.interpolating = False
     self.playing = False
     self.servoCount = 12
     self.lastFrame = pyb.millis()
     self.port = UART_Port(1, 1000000)
     self.bus = Bus(self.port, show=Bus.SHOW_PACKETS)
     # Bus.SHOW_NONE
     # Bus.SHOW_COMMANDS
     # Bus.SHOW_PACKETS
     if useLogger:
         self.logger = Logger('sync_log.txt')
     else:
         self.logger = None
Exemple #5
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)
Exemple #6
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)
Exemple #7
0
class BioloidController:

    def __init__(self, useLogger = False):
        self.id = [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12]
        self.pose = [512, 512, 512, 512, 512, 512, 512, 512, 512, 512, 512, 512]
        self.nextPose = [512, 512, 512, 512, 512, 512, 512, 512, 512, 512, 512, 512]
        self.speed = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
        self.interpolating = False
        self.playing = False
        self.servoCount = 12
        self.lastFrame = pyb.millis()
        self.port = UART_Port(1, 1000000)
        self.bus = Bus(self.port, show=Bus.SHOW_PACKETS)
        # Bus.SHOW_NONE
        # Bus.SHOW_COMMANDS
        # Bus.SHOW_PACKETS
        if useLogger:
            self.logger = Logger('sync_log.txt')
        else:
            self.logger = None

    # Load a pose into nextPose
    def loadPose(self, poseArray):
        for i in range(self.servoCount):
            self.nextPose[i] = (poseArray[i]) # << BIOLOID_SHIFT)
            #print ('loadPose[', self.id[i], '] = ', self.nextPose[i])

    def isLogging(self):
        return self.logger is not None

    # read the current robot's pose
    def readPose(self):
        for i in range(self.servoCount):
            self.pose[i] = (self.readTwoByteRegister(self.id[i], AX_GOAL_POSITION)) # << BIOLOID_SHIFT)
            #print ('readPose[', self.id[i], '] = ', self.pose[i])
            pyb.delay(25)

    def writePose(self):
        values = []
        logging = self.isLogging()
        if logging:
            logValues = []
        for i in range(self.servoCount):
            poseValue = int(self.pose[i])
            values.append(struct.pack('<H', poseValue))
            if logging:
                logValues.append(poseValue)
        self.bus.sync_write(self.id, AX_GOAL_POSITION, values)
        if logging:
            self.logger.log(logValues)

    def slowMoveServoTo(self, deviceId, targetPosition, speed = SLOW_SERVO_MOVE_SPEED, scanFunction = None):
        oldSpeed = self.readTwoByteRegister(deviceId, AX_MOVING_SPEED)
        currentPosition = self.readTwoByteRegister(deviceId, AX_PRESENT_POSITION)
        self.writeTwoByteRegister(deviceId, AX_MOVING_SPEED, speed)
        self.writeTwoByteRegister(deviceId, AX_GOAL_POSITION, targetPosition)
        done = False
        scanCount = 0
        lastPosition = 0
        startTime = pyb.millis()
        while abs(currentPosition - targetPosition) > 5:
            currentPosition = self.readTwoByteRegister(deviceId, AX_PRESENT_POSITION)
            if scanFunction is not None:
                if currentPosition != lastPosition:
                    scanCount += 1
                    lastPosition = currentPosition
                    scanFunction(currentPosition, scanCount)
            pyb.delay(1)
        self.writeTwoByteRegister(deviceId, AX_MOVING_SPEED, oldSpeed)
        if scanFunction is not None:
            scanFunction(targetPosition, scanCount + 1)
        print("Elapsed Time: %d" % (pyb.millis() - startTime))

    def rampServoTo(self, deviceId, targetPosition):
        currentPosition = self.readTwoByteRegister(deviceId, AX_PRESENT_POSITION) # present position
        if targetPosition > currentPosition:
            stepDelta = 1
            stepAccel = 2
            comparison = lambda: targetPosition > (currentPosition + stepDelta)
        else:
            stepDelta = -1
            stepAccel = -2
            comparison = lambda: currentPosition > (targetPosition - stepDelta)
        while comparison():
            movePosition = currentPosition + stepDelta
            stepDelta += stepAccel
            self.setPosition(deviceId, movePosition)
            currentPosition = self.readTwoByteRegister(deviceId, AX_PRESENT_POSITION) # present position
            pyb.delay(25)
        self.setPosition(deviceId, targetPosition)

    def setPosition(self, deviceId, position):
        self.writeTwoByteRegister(deviceId, AX_GOAL_POSITION, position)

    def writeData(self, deviceId, controlTableIndex, byteData):
        try:
            result = self.bus.write(deviceId, controlTableIndex, byteData)
        except BusError as ex:
            if ex.get_error_code() == ErrorCode.CHECKSUM:
                print ("CHECKSUM Error - retrying...")
                return self.bus.write(deviceId, controlTableIndex, byteData)
            raise
        return result

    def writeTwoByteRegister(self, deviceId, controlTableIndex, value):
        return self.writeData(deviceId, controlTableIndex, struct.pack('<H', value))

    def writeOneByteRegister(self, deviceId, controlTableIndex, value):
        return self.writeData(deviceId, controlTableIndex, struct.pack('B', value))

    def readTwoByteRegister(self, deviceId, controlTableIndex):
        values = self.readData(deviceId, controlTableIndex, 2)
        return struct.unpack('<H', values)[0]

    def readOneByteRegister(self, deviceId, controlTableIndex):
        values = self.readData(deviceId, controlTableIndex, 1)
        return struct.unpack('B', values)[0]

    def readData(self, deviceId, controlTableIndex, count):
        try:
            result = self.bus.read(deviceId, controlTableIndex, count)
        except BusError as ex:
            if ex.get_error_code() == ErrorCode.CHECKSUM:
                print ("CHECKSUM Error - retrying...")
                return self.bus.read(deviceId, controlTableIndex, count)
            raise
        return result

    def interpolateSetup(self, time):
        frames = (time / BIOLOID_FRAME_LENGTH) + 1
        self.lastFrame = pyb.millis()
        for i in range(self.servoCount):
            if self.nextPose[i] > self.pose[i]:
                self.speed[i] = (self.nextPose[i] - self.pose[i]) / frames + 1
            else:
                self.speed[i] = (self.pose[i] - self.nextPose[i]) / frames + 1
        self.interpolating = True

    def interpolateStep(self):
        if not self.interpolating:
            return
        complete = self.servoCount
        while (pyb.millis() - self.lastFrame < BIOLOID_FRAME_LENGTH):
            pass
        self.lastFrame = pyb.millis()
        for i in range(self.servoCount):
            diff = self.nextPose[i] - self.pose[i]
            if diff == 0:
                complete -= 1
            else:
                if diff > 0:
                    if diff < self.speed[i]:
                        self.pose[i] = self.nextPose[i]
                        complete -= 1
                    else:
                        self.pose[i] += self.speed[i]
                else:
                    if (-diff) < self.speed[i]:
                        self.pose[i] = self.nextPose[i]
                        complete -= 1
                    else:
                        self.pose[i] -= self.speed[i]
        if complete <= 0:
            self.interpolating = False
        self.writePose()
Exemple #8
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)
Exemple #9
0
class BioloidController:

    def __init__(self, useLogger = False):
        self.id = [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12]
        self.pose = [512, 512, 512, 512, 512, 512, 512, 512, 512, 512, 512, 512]
        self.nextPose = [512, 512, 512, 512, 512, 512, 512, 512, 512, 512, 512, 512]
        self.speed = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
        self.interpolating = False
        self.playing = False
        self.servoCount = 12
        self.lastFrame = pyb.millis()
        self.bus = UART_Bus(1, 1000000, show_packets=False)
        if useLogger:
            self.logger = Logger('sync_log.txt')
        else:
            self.logger = None

    # Load a pose into nextPose
    def loadPose(self, poseArray):
        for i in range(self.servoCount):
            self.nextPose[i] = (poseArray[i]) # << BIOLOID_SHIFT)
            #print ('loadPose[', self.id[i], '] = ', self.nextPose[i])

    def isLogging(self):
        return self.logger is not None

    # read the current robot's pose
    def readPose(self):
        for i in range(self.servoCount):
            self.pose[i] = (self.readTwoByteRegister(self.id[i], AX_GOAL_POSITION)) # << BIOLOID_SHIFT)
            #print ('readPose[', self.id[i], '] = ', self.pose[i])
            pyb.delay(25)

    def writePose(self):
        values = []
        logging = self.isLogging()
        if logging:
            logValues = []
        for i in range(self.servoCount):
            poseValue = int(self.pose[i])
            values.append(struct.pack('<H', poseValue))
            if logging:
                logValues.append(poseValue)
        self.bus.sync_write(self.id, AX_GOAL_POSITION, values)
        if logging:
            self.logger.log(logValues)

    def slowMoveServoTo(self, deviceId, targetPosition, speed = SLOW_SERVO_MOVE_SPEED, scanFunction = None):
        oldSpeed = self.readTwoByteRegister(deviceId, AX_MOVING_SPEED)
        currentPosition = self.readTwoByteRegister(deviceId, AX_PRESENT_POSITION)
        self.writeTwoByteRegister(deviceId, AX_MOVING_SPEED, speed)
        self.writeTwoByteRegister(deviceId, AX_GOAL_POSITION, targetPosition)
        done = False
        scanCount = 0
        lastPosition = 0
        startTime = pyb.millis()
        while abs(currentPosition - targetPosition) > 5:
            currentPosition = self.readTwoByteRegister(deviceId, AX_PRESENT_POSITION)
            if scanFunction is not None:
                if currentPosition != lastPosition:
                    scanCount += 1
                    lastPosition = currentPosition
                    scanFunction(currentPosition, scanCount)
            pyb.delay(1)
        self.writeTwoByteRegister(deviceId, AX_MOVING_SPEED, oldSpeed)
        if scanFunction is not None:
            scanFunction(targetPosition, scanCount + 1)
        print("Elapsed Time: %d" % (pyb.millis() - startTime))

    def rampServoTo(self, deviceId, targetPosition):
        currentPosition = self.readTwoByteRegister(deviceId, AX_PRESENT_POSITION) # present position
        if targetPosition > currentPosition:
            stepDelta = 1
            stepAccel = 2
            comparison = lambda: targetPosition > (currentPosition + stepDelta)
        else:
            stepDelta = -1
            stepAccel = -2
            comparison = lambda: currentPosition > (targetPosition - stepDelta)
        while comparison():
            movePosition = currentPosition + stepDelta
            stepDelta += stepAccel
            self.setPosition(deviceId, movePosition)
            currentPosition = self.readTwoByteRegister(deviceId, AX_PRESENT_POSITION) # present position
            pyb.delay(25)
        self.setPosition(deviceId, targetPosition)

    def setPosition(self, deviceId, position):
        self.writeTwoByteRegister(deviceId, AX_GOAL_POSITION, position)

    def writeData(self, deviceId, controlTableIndex, byteData):
        try:
            result = self.bus.write(deviceId, controlTableIndex, byteData)
        except BusError as ex:
            if ex.get_error_code() == ErrorCode.CHECKSUM:
                print ("CHECKSUM Error - retrying...")
                return self.bus.write(deviceId, controlTableIndex, byteData)
            raise
        return result

    def writeTwoByteRegister(self, deviceId, controlTableIndex, value):
        return self.writeData(deviceId, controlTableIndex, struct.pack('<H', value))

    def writeOneByteRegister(self, deviceId, controlTableIndex, value):
        return self.writeData(deviceId, controlTableIndex, struct.pack('B', value))

    def readTwoByteRegister(self, deviceId, controlTableIndex):
        values = self.readData(deviceId, controlTableIndex, 2)
        return struct.unpack('<H', values)[0]

    def readOneByteRegister(self, deviceId, controlTableIndex):
        values = self.readData(deviceId, controlTableIndex, 1)
        return struct.unpack('B', values)[0]

    def readData(self, deviceId, controlTableIndex, count):
        try:
            result = self.bus.read(deviceId, controlTableIndex, count)
        except BusError as ex:
            if ex.get_error_code() == ErrorCode.CHECKSUM:
                print ("CHECKSUM Error - retrying...")
                return self.bus.read(deviceId, controlTableIndex, count)
            raise
        return result

    def interpolateSetup(self, time):
        frames = (time / BIOLOID_FRAME_LENGTH) + 1
        self.lastFrame = pyb.millis()
        for i in range(self.servoCount):
            if self.nextPose[i] > self.pose[i]:
                self.speed[i] = (self.nextPose[i] - self.pose[i]) / frames + 1
            else:
                self.speed[i] = (self.pose[i] - self.nextPose[i]) / frames + 1
        self.interpolating = True

    def interpolateStep(self):
        if not self.interpolating:
            return
        complete = self.servoCount
        while (pyb.millis() - self.lastFrame < BIOLOID_FRAME_LENGTH):
            pass
        self.lastFrame = pyb.millis()
        for i in range(self.servoCount):
            diff = self.nextPose[i] - self.pose[i]
            if diff == 0:
                complete -= 1
            else:
                if diff > 0:
                    if diff < self.speed[i]:
                        self.pose[i] = self.nextPose[i]
                        complete -= 1
                    else:
                        self.pose[i] += self.speed[i]
                else:
                    if (-diff) < self.speed[i]:
                        self.pose[i] = self.nextPose[i]
                        complete -= 1
                    else:
                        self.pose[i] -= self.speed[i]
        if complete <= 0:
            self.interpolating = False
        self.writePose()