def Penalized(): global phase global firstCall if firstCall['Penalized']: print 'In penalized state' visThread.stopScan() # stop looking for the ball mot.killWalk() # stop walking mot.stiff() # stiffness on (just in case?) mot.keepNormalPose() ledProxy.fadeRGB('ChestLeds',0x00ff0000, 0) # ledProxy on chest: red if playerType == 0: # if a player, goto unpenalized phase phase = 'Unpenalized' else: # if a keeper, go to ballnotfoundkeep if penalty shootout if sc.getSecondaryState(): phase = 'BallNotFoundKeep' else: # else, become a player phase = 'Unpenalized' firstCall['Initial'] = True firstCall['Ready'] = True firstCall['Set'] = True firstCall['Playing'] = True firstCall['Penalized'] = False
def run(samples = 100): motions.stiff() motions.normalPose() timeStamp = time.time() checkLeft = True checkRight = True speech.say("Hello, I am overmind") while(True): (left, right) = getData(samples) print "Right:", right, "Left:", left if (wall(left, right)): print "Wall detected" if (checkLeft): print "Checking right" motions.walkTo((0, 0, -1.815)) checkLeft = False elif (checkRight): print "Checking left" motions.walkTo((0, 0, 2.78)) checkRight = False speech.say("I have escaped this maze like a boss") else: print "Checking back" motions.walkTo((0, 0, 1.39)) else: print "Walking straight" motions.SWTV((0.5, 0, -0.0525, 0.8)) checkLeft = True checkRight = True
def Ready(): """Ready state: possible positioning ledProxy: Chest Blue """ global firstCall # Reinitialize phases, stand ready for match if firstCall["Ready"]: # stop ballfinding, there is no ball anyway visThread.stopScan() print "In ready state" mHandler.killWalk() # Initial pose mot.stiff() if playerType == 0: mot.normalPose() else: mot.keepNormalPose() firstCall["Initial"] = True firstCall["Ready"] = False firstCall["Set"] = True firstCall["Playing"] = True firstCall["Penalized"] = True firstCall["FirstPress"] = True
def Set(): global control global teamColor global kickOff global phase global firstCall control = [0,0,0] # if the first iteration if firstCall['Set']: # update info about team, it is possible that game is started in set phase (teamColor, kickOff, penalty) = sc.getMatchInfo() print 'In set state' visThread.startScan() # start ballscan print 'TeamColor: ' , teamColor # print teamcolor as a check # Initial pose, if not already in it mot.stiff() if playerType == 0: mot.normalPose() else: mot.keepNormalPose() audProxy.setOutputVolume(0) # set volume to zero ledProxy.fadeRGB('ChestLeds', 0x00ffff00, 0) # set chestledcolor to green # Team color must be displayed on left foot (Reference to rules) if (teamColor == 0): ledProxy.fadeRGB('LeftFootLeds', 0x000000ff, 0) else: ledProxy.fadeRGB('LeftFootLeds', 0x00ff0000, 0) firstCall['Initial'] = True firstCall['Ready'] = True firstCall['Set'] = False firstCall['Playing'] = True firstCall['Penalized'] = True # Head movements allowed in the set state # FIND A BALL # if phase != 'BallFound' and phase != 'BallFoundKeep': if vis.scanCircle(visThread, 0.2): if playerType == 0: phase = 'BallFound' else: phase = 'BallFoundKeep' # Keep finding the ball, it might move (or does it?) if not(visThread.findBall()): if playerType == 0: phase = 'BallNotFound' else: phase = 'BallNotFoundKeep'
def Playing(): """Playing state: play game according to phase transitions ledProxy: Chest Green """ global phase global firstCall if firstCall["Set"]: mot.stiff() # stiffness on (if it wasnt already) mot.setHead(0, 0) # head in normal position # if a regular player if playerType == 0: phase = "BallNotFound" mot.normalPose(True) # normalPose, forcing call # if a keeper else: phase = "BallNotFoundKeep" mot.keepNormalPose() firstCall["Set"] = False if firstCall["Playing"]: mHandler.killWalk() visThread.startScan() print "In playing state" firstCall["Initial"] = True firstCall["Ready"] = True firstCall["Set"] = True firstCall["Playing"] = False firstCall["Penalized"] = True if mot.standUp(): print "Fallen" try: if memProxy.getData("dntAction"): phase = memProxy.getData("dntAction") print "coach says: " + phase else: print "coach says: nothing" except: pass # Execute the phase as specified by phase variable phases.get(phase)()
def Set(): """Set state: start searching for ball. CAUTION: Game is started in Set phase instead of Initial in penalty shootout! ledProxy: Chest Yellow """ global teamColor global kickOff global phase global firstCall # if the first iteration if firstCall["Set"]: mHandler.killWalk() # update info about team, it is possible that game is started in set phase (teamColor, kickOff, penalty) = gsc.getMatchInfo() print "In set state" visThread.startScan() # start ballscan print "TeamColor: ", teamColor # print teamcolor as a check # Initial pose, if not already in it mot.stiff() if playerType == 0: mot.normalPose() else: mot.keepNormalPose() # audProxy.setOutputVolume(0) # set volume to zero firstCall["Initial"] = True firstCall["Ready"] = True firstCall["Set"] = False firstCall["Playing"] = True firstCall["Penalized"] = True # Head movements allowed in the set state # FIND A BALL # ball = vis.scanCircle(visThread) if ball: mHandler.setBallLoc(ball) if playerType == 0: phase = "BallFound" else: phase = "BallFoundKeep"
def Initial(): global phase # to change, call global, else not possible global ball_loc global firstCall if firstCall['Initial']: print 'In initial state' visThread.stopScan() # do not find the ball when in Initial print 'TeamColor: ' , teamColor # print the teamcolor as a check # Empty variables ball_loc = dict() # Team color must be displayed on left foot (Reference to rules) ledProxy.off('AllLeds') if (teamColor == 0): ledProxy.fadeRGB('LeftFootLeds', 0x000000ff, 0) else: ledProxy.fadeRGB('LeftFootLeds', 0x00ff0000, 0) mot.stiff() # stiffness on (if it wasnt already) mot.setHead(0,0) # head in normal position # if a regular player if playerType == 0: phase = 'BallNotFound' mot.normalPose(True) # normalPose, forcing call # if a keeper else: phase = 'BallNotFoundKeep' mot.keepNormalPose() firstCall['Initial'] = False firstCall['Ready'] = True firstCall['Set'] = True firstCall['Playing'] = True firstCall['Penalized'] = True
def run(samples=100): motions.stiff() motions.normalPose() timeStamp = time.time() check = True while (time.time() - timeStamp < 1000): (left, right) = getData(samples) print "Right:", right, "Left:", left deltaL = left - right deltaR = right - left print "DeltaR:", deltaR, "DeltaL:", deltaL if ((left < 40) and (right < 40)): print "WALL DETECTED" if (check): print "CHECK RIGHT" motions.walkTo((0, 0, -1.33)) check = False else: print "CHECK LEFT" motions.SWTV((0, 0, 0.25, 0.7)) elif ((right >= 40) and (left >= 40)): check = True print "WALKING STRAIGHT" motions.SWTV((0.5, 0, 0, 0.5)) elif ((deltaR < 0 and deltaR > -5)): print "ADJUST ANGLE TO LEFT" motions.SWTV((0.5, 0, 0.05, 0.5)) elif ((deltaL < 0 and deltaL > -5)): print "ADJUST ANGLE TO RIGHT" motions.SWTV((0.5, 0, -0.05, 0.5)) else: print "STOP" motions.walkTo((0.01, 0, 0)) print "STOP" motions.walkTo((0.01, 0, 0))