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 __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
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
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
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)
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)
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()
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)
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()