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 GameStatus(self):
self.DetermineGameState()
if self.state == 'MainMenu':
self.menu.MainMenu(self.screen)
elif self.state == 'TuteMenu':
self.menu.TutorialMenu(self.screen)
elif self.state == 'SelectAI':
self.menu.SelectAI(self.screen)
elif self.state == 'FSM':
self.player = Player('AI')
self.fsm = FiniteStateMachine(self)
self.fsm.RunGameFSM()
self.menu.SetState('MainMenu')
elif self.state == 'PlayGameHuman':
self.player = Player('HUMAN')
self.level = Level(self.screen, self.player)
self.RunGameHuman()
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 main():
G = Grammar(grammar)
if G.grammarType[0] != 3:
print "Not a regular grammar!\n"
return
FSM = FiniteStateMachine(G, "K", ["V"])
drawFSM(FSM, "FSM_initial")
FSM.removeUnreachableStates()
FSM.determinate()
drawFSM(FSM, "FSM_determinated")
FSM.printRules()
def main():
currentGrammar = grammars[gramarToUse]
G = Grammar(currentGrammar["grammar"])
if G.grammarType[0] != 3:
print "Not a regular grammar!\n"
return
FSM = FiniteStateMachine(G, currentGrammar["startState"],
currentGrammar["endStates"])
drawFSM(FSM, "FSM_initial")
FSM.removeUnreachableStates()
drawFSM(FSM, "FSM_no_unreachable_states")
FSM.minimize()
drawFSM(FSM, "FSM_minimised")
FSM.printRules()
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)
def __init__(self):
""" init """
self.keypad_pointer = Keypad()
self.led_board_pointer = LEDBoard()
FiniteStateMachine(self)
class World():
# Initialisation
def __init__(self):
#self.player = Player()
self.ScreenSetup()
self.menu = Menus(self.screen)
self.text = Text(self.screen)
self.state = ''
self.playing = True
self.clock = pygame.time.Clock()
self.fps = 120
self.obstacleSprites = []
self.obList = []
self.collision = CollisionChecker()
def RunningStatus(self):
if self.state == 'Quit':
return False
else:
return True
# Sets up the screen
def ScreenSetup(self):
width = 1000
height = 800
self.screen = pygame.display.set_mode(
(width, height), pygame.HWSURFACE | pygame.DOUBLEBUF)
pygame.display.set_caption('Dodge The Monsters 3')
# Draws all the game objects
def GameStatus(self):
self.DetermineGameState()
if self.state == 'MainMenu':
self.menu.MainMenu(self.screen)
elif self.state == 'TuteMenu':
self.menu.TutorialMenu(self.screen)
elif self.state == 'SelectAI':
self.menu.SelectAI(self.screen)
elif self.state == 'FSM':
self.player = Player('AI')
self.fsm = FiniteStateMachine(self)
self.fsm.RunGameFSM()
self.menu.SetState('MainMenu')
elif self.state == 'PlayGameHuman':
self.player = Player('HUMAN')
self.level = Level(self.screen, self.player)
self.RunGameHuman()
def DetermineGameState(self):
self.state = self.menu.GetState()
def LoadObstacles(self):
self.obList = [
Obstacle(130, -100, 1),
Obstacle(350, -100, 2),
Obstacle(645, -100, 3),
Obstacle(800, -100, 4)
]
# Handles the left and right movement of the player
def HandlePlayerMovement(self):
if self.player.type == 'HUMAN':
if pygame.key.get_pressed()[pygame.K_LEFT]:
self.player.MoveLeft()
if pygame.key.get_pressed()[pygame.K_RIGHT]:
self.player.MoveRight()
if not pygame.key.get_pressed(
)[pygame.K_LEFT] and not pygame.key.get_pressed()[pygame.K_RIGHT]:
self.player.ReturnStraight()
def GetSprites(self):
self.allObstacles = pygame.sprite.Group()
for o in self.obList:
self.allObstacles.add(o.oSprite)
def AddObstacle(self):
if self.level.level == 2 and len(self.obList) == 4:
self.obList.append(
Obstacle(random.randint(130, 850), (random.randint(50, 300)),
5))
elif self.level.level == 3 and len(self.obList) == 5:
self.obList.append(
Obstacle(random.randint(130, 850), (random.randint(50, 300)),
6))
elif self.level.level == 4 and len(self.obList) == 6:
self.obList.append(
Obstacle(random.randint(130, 850), (random.randint(50, 300)),
7))
def RunGameHuman(self):
self.LoadObstacles()
self.game = GameLogic(self, self.screen, self.level, self.text)
self.GetSprites()
self.player.DrawPlayer(self.screen)
for o in self.obList:
o.DrawObstacle(self.screen)
self.menu.StartGame(self.screen)
while self.playing and not self.collision.CheckCollision(
self.obList, self.player):
# Get the player movement
self.HandlePlayerMovement()
# Add other Obstacles
self.AddObstacle()
# Run the game logic
self.game.TheGame(self.obList)
# Refresh Rate
self.clock.tick(120)
pygame.time.delay(1000)
self.menu.EndGameScreen(self.screen, self.player)
pygame.time.delay(5000)
self.menu.SetState('MainMenu')
def run(self):
text = self.text
FSM = FiniteStateMachine()
FSM.startFSM()
return FSM.runFSM(text)
def firstStateUpdate(): print 'first state update' stateMachine.transitionTo(secondState) print 'first state update done' def firstStateExit(): print 'first state exit' def secondStateEnter(): print 'second state enter' def secondStateUpdate(): print 'second state update' stateMachine.transitionTo(thirdState) print 'second state update done' def thirdStateUpdate(): print 'third state update' firstState = State(firstStateEnter, firstStateUpdate, firstStateExit) secondState = State(secondStateEnter, secondStateUpdate, None) thirdState = State(None, thirdStateUpdate, None) stateMachine = FiniteStateMachine(firstState) while True: print 'state machine pre-update' stateMachine.update() print 'state machine post-update' time.sleep(0.5)
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)
def firstStateExit():
print 'first state exit'
def secondStateEnter():
print 'second state enter'
def secondStateUpdate():
print 'second state update'
stateMachine.transitionTo(thirdState)
print 'second state update done'
def thirdStateUpdate():
print 'third state update'
firstState = State(firstStateEnter, firstStateUpdate, firstStateExit)
secondState = State(secondStateEnter, secondStateUpdate, None)
thirdState = State(None, thirdStateUpdate, None)
stateMachine = FiniteStateMachine(firstState)
while True:
print 'state machine pre-update'
stateMachine.update()
print 'state machine post-update'
time.sleep(0.5)