def run(self): while True: self.sensors.camera.update() if self.timer.millis() > 100: self.sensors.update() if self.stateTimeout.isTimeUp( ) == True or self.isClearOfSurroundings(): print 'Breaking Free is finished. Returning to startState...' return startState.startState(self.sensors, self.actuators, self.motorController, self.timer, self.utils) if self.substate == "backUp": if self.backUpTimeout.isTimeUp(): print 'Done backing up. Now turning...' self.substate = "Turning" if self.isLeftFree(): self.turnNDegreesSlowly(random.randrange(60, 120)) else: self.turnNDegreesSlowly(random.randrange( -120, -60)) else: self.driveStraight(self.BACK_UP_SPEED) if self.substate == "Turning": if self.isFinishedTurning(): return startState.startState(self.sensors, self.actuators, self.motorController, self.timer, self.utils) self.actuators.update() self.motorController.updateMotorSpeeds() self.timer.reset()
def run(self): while True: self.sensors.camera.update() if self.timer.millis() > 100: self.sensors.update() if self.stateTimeout.isTimeUp() == True or self.isClearOfSurroundings(): print 'Breaking Free is finished. Returning to startState...' return startState.startState(self.sensors, self.actuators, self.motorController, self.timer, self.utils) if self.substate == "backUp": if self.backUpTimeout.isTimeUp(): print 'Done backing up. Now turning...' self.substate = "Turning" if self.isLeftFree(): self.turnNDegreesSlowly(random.randrange(60,120)) else: self.turnNDegreesSlowly(random.randrange(-120,-60)) else: self.driveStraight(self.BACK_UP_SPEED) if self.substate == "Turning": if self.isFinishedTurning(): return startState.startState(self.sensors, self.actuators, self.motorController, self.timer, self.utils) self.actuators.update() self.motorController.updateMotorSpeeds() self.timer.reset()
def run(self): while True: self.sensors.camera.update() if self.timer.millis() > 100: self.sensors.update() self.wallFollow("Right", self.WALL_FOLLOW_SPEED, self.wallFollowPID) #check timeout and block posession if self.timeout.isTimeUp() == True: print 'Done with blind wall following.' return startState.startState(self.sensors, self.actuators, self.motorController, self.timer, self.utils) if self.sensors.uIR.val == 0: print 'Break beam has sensed a block. Going to Pick Up Block State...' return pickUpBlockState.PickUpBlockState( self.sensors, self.actuators, self.motorController, self.timer, self.utils) self.actuators.update() self.motorController.updateMotorSpeeds() self.timer.reset()
def setup(self): self.timer = Timer() self.utils = Utils(time.time(),180) #start time, total game time self.sensors = Sensors(self.tamp) self.actuators = Actuators(self.tamp) self.motorController= MotorController(self.sensors,self.actuators) self.myState = startState(self.sensors, self.actuators, self.motorController, self.timer, self.utils)
def setup(self): self.timer = Timer() self.utils = Utils(time.time(), 180) #start time, total game time self.sensors = Sensors(self.tamp) self.actuators = Actuators(self.tamp) self.motorController = MotorController(self.sensors, self.actuators) self.myState = startState(self.sensors, self.actuators, self.motorController, self.timer, self.utils)
def run(self): while True: self.sensors.camera.update() if self.timer.millis() > 100: self.sensors.update() #State Timeout and break-beam detection if self.timeout.isTimeUp() == True: print 'State Timeout has timed out. Going to BreakFreeState...' return breakFreeState.BreakFreeState( self.sensors, self.actuators, self.motorController, self.timer, self.utils) if self.sensors.uIR.val == 0: print 'Break beam has sensed a block. Going to Pick Up Block State...' return pickUpBlockState.PickUpBlockState( self.sensors, self.actuators, self.motorController, self.timer, self.utils) #TODO: what if when the state starts, we are closer to the block than the CLOSE_ENOUGH_DISTANCE? That will cause changes needed in the EatBlock state collision code. #TODO: (minor) there is code duplication between collision code sections of ApproachBlock and EatBlock code if self.substate == "ApproachBlock": print 'Starting Approach...' #self.driveStraight(self.DRIVE_SPEED) self.turnNDegreesAndMove(self.sensors.camera.blockAngle, self.DRIVE_SPEED) if self.sensors.camera.detectBlock == False: print 'Cannot see block anymore. May be too close. Will try to eat it.' return eatBlockState.EatBlockState( self.sensors, self.actuators, self.motorController, self.timer, self.utils) elif self.isColliding(): canOvercomeCollision = self.dealWithCollision( ) #dealWithCollision will take care of changing the substate if canOvercomeCollision == False: print 'Area too cramped to overcome collision. Perhaps we are in a corner? Fall back to strict wall following...' return blindWallFollowingState.BlindWallFollowingState( self.sensors, self.actuators, self.motorController, self.timer, self.utils) elif self.substate == "FlankManeuver": self.performFlankManeuver() elif self.substate == "FlankFailed": print 'Flank Failed. Going to blind wall follow' return blindWallFollowingState.BlindWallFollowingState( self.sensors, self.actuators, self.motorController, self.timer, self.utils) else: print 'Error! Substate named ', self.substate, ' was not recognized. Exiting to Start State...' return startState.startState(self.sensors, self.actuators, self.motorController, self.timer, self.utils) self.actuators.update() self.motorController.updateMotorSpeeds() self.timer.reset()
def run(self): while True: self.sensors.camera.update() if self.timer.millis() > 100: self.sensors.update() #constantly check to see if our state has timed out if self.timeout.isTimeUp() == True: print 'State has timed out! Exiting to start state...' return startState.startState(self.sensors, self.actuators, self.motorController, self.timer, self.utils) #constantly check to see if we have acquired a block if self.sensors.uIR.val == 0: print 'Detected a block captured! Now picking it up...' self.driveStraight(0) self.turnConstantRate(0) return pickUpBlockState.PickUpBlockState(self.sensors, self.actuators, self.motorController, self.timer, self.utils) #constanlty check to see if we have detected a new block if self.sensors.camera.detectBlock == True: self.driveStraight(0) self.turnConstantRate(0) return turnToBlockState.TurnToBlockState(self.sensors, self.actuators, self.motorController, self.timer, self.utils) if self.substate == "moveForward": self.driveStraight(self.DRIVE_SPEED) if self.sensors.encoders.getDistanceTraveled() >= self.FORWARD_DISTANCE or self.isColliding(): print 'Either finished moving forward or about to crash. Now moving backwards.' self.substate == "backUp" self.sensors.encoders.resetEncoders() if self.substate == "backUp": self.driveStraight(self.BACK_UP_SPEED) if self.sensors.encoders.getDistanceTraveled() <= self.BACKWARD_DISTANCE or self.isCollidingOnSides(): print 'Finished backing up or detecting potential collision with slanted wall. Now exiting state.' self.driveStraight(0) self.turnConstantRate(0) return startState.startState(self.sensors, self.actuators, self.motorController, self.timer, self.utils) self.actuators.update() self.motorController.updateMotorSpeeds() self.timer.reset()
def loop(self): try: self.myState=self.myState.run() #run current state and return next state except KeyboardInterrupt: raise except: print "cought an error, back to start state" self.myState=startState(self.sensors, self.actuators, self.motorController, self.timer, self.utils) self.timer.reset()
def loop(self): try: self.myState = self.myState.run( ) #run current state and return next state except KeyboardInterrupt: raise except: print "cought an error, back to start state" self.myState = startState(self.sensors, self.actuators, self.motorController, self.timer, self.utils) self.timer.reset()
def run(self): while True: self.sensors.camera.update() if self.timer.millis() > 100: self.sensors.update() #State Timeout and break-beam detection if self.timeout.isTimeUp() == True: print 'State Timeout has timed out. Going to BreakFreeState...' return breakFreeState.BreakFreeState(self.sensors, self.actuators, self.motorController, self.timer, self.utils) if self.sensors.uIR.val == 0: print 'Break beam has sensed a block. Going to Pick Up Block State...' return pickUpBlockState.PickUpBlockState(self.sensors, self.actuators, self.motorController, self.timer, self.utils) if self.substate == "EatBlock": self.driveStraight(self.DRIVE_SPEED) if self.isColliding(): status_line = 'Collision Info: ' for x in xrange(6): status_line +='Sensor ' status_line += str(x) status_line += " :" status_line += "{:6.2f}".format(self.sensors.irArray.ir_value[x]) status_line += " " print status_line self.driveStraight(0) self.eat_distance -= self.sensors.encoders.getDistanceTraveled() self.substate = "DragBlock" elif self.sensors.encoders.getDistanceTraveled() >= self.eat_distance: print 'Finished attempt to eat block. Break beam did not go off.' return lookingForBlocksState.LookingForBlocksState(self.sensors, self.actuators, self.motorController, self.timer, self.utils) elif self.substate == "DragBlock": if self.isColliding() == False: print 'Found an opening we can move through.' self.turnConstantRate(0,"Right") #redundant? self.sensors.encoders.resetEncoders() self.substate = "EatBlock" elif self.sensors.gyro.gyroCAngle >= (self.start_gyro_angle + 360): print 'After a full 360, could not find a good position about which to turn. Begin blind wall following...' return blindWallFollowingState.BlindWallFollowingState(self.sensors, self.actuators, self.motorController, self.timer, self.utils) else: self.turnConstantRate(self.DRAG_TURN_RATE,"Right") else: print 'Error! Substate named ', self.substate, ' was not recognized. Exiting to Start State...' return startState.startState(self.sensors, self.actuators, self.motorController, self.timer, self.utils) self.actuators.update() self.motorController.updateMotorSpeeds() self.timer.reset()
def run(self): while True: self.sensors.camera.update() if self.timer.millis() > 100: self.sensors.update() #State Timeout and break-beam detection if self.timeout.isTimeUp() == True: print 'State Timeout has timed out. Going to BreakFreeState...' return breakFreeState.BreakFreeState(self.sensors, self.actuators, self.motorController, self.timer, self.utils) if self.sensors.uIR.val == 0: print 'Break beam has sensed a block. Going to Pick Up Block State...' return pickUpBlockState.PickUpBlockState(self.sensors, self.actuators, self.motorController, self.timer, self.utils) #TODO: what if when the state starts, we are closer to the block than the CLOSE_ENOUGH_DISTANCE? That will cause changes needed in the EatBlock state collision code. #TODO: (minor) there is code duplication between collision code sections of ApproachBlock and EatBlock code if self.substate == "ApproachBlock": print 'Starting Approach...' #self.driveStraight(self.DRIVE_SPEED) self.turnNDegreesAndMove(self.sensors.camera.blockAngle,self.DRIVE_SPEED) if self.sensors.camera.detectBlock == False: print 'Cannot see block anymore. May be too close. Will try to eat it.' return eatBlockState.EatBlockState(self.sensors, self.actuators, self.motorController, self.timer, self.utils) elif self.isColliding(): canOvercomeCollision = self.dealWithCollision() #dealWithCollision will take care of changing the substate if canOvercomeCollision == False: print 'Area too cramped to overcome collision. Perhaps we are in a corner? Fall back to strict wall following...' return blindWallFollowingState.BlindWallFollowingState(self.sensors, self.actuators, self.motorController, self.timer, self.utils) elif self.substate == "FlankManeuver": self.performFlankManeuver() elif self.substate == "FlankFailed": print 'Flank Failed. Going to blind wall follow' return blindWallFollowingState.BlindWallFollowingState(self.sensors, self.actuators, self.motorController, self.timer, self.utils) else: print 'Error! Substate named ', self.substate, ' was not recognized. Exiting to Start State...' return startState.startState(self.sensors, self.actuators, self.motorController, self.timer, self.utils) self.actuators.update() self.motorController.updateMotorSpeeds() self.timer.reset()
def __init__(self, stateScriptPath, ioManifestLocation, extPreProcessor=None, extPostProcessor=None): sys.path.insert(1, os.path.abspath(os.getcwd()) + stateScriptPath) #may want a try block here try: from startState import startState except: print("Invalid relative stateScript location") STACKSIZE = 10 #could be much larger for actual use self.Continue = "Good" ## create machine objects ## #outProcessor = Postprocessor() #default recieve/publish objects self.inProcessor = Preprocessor(ioManifestLocation) self.currentState = startState() #default state self.inputVector = {} #fsm input language self.outputVector = {} #fsm output language self.pushDownHistory = History(STACKSIZE) ## check in processor ## if ( extPreProcessor != None ): #replace the pre-processor object with a test pre-processor if it is provided self.inProcessor = extPreProcessor if ( extPostProcessor != None ): #replace the pre-processor object with a test pre-processor if it is provided self.outProcessor = extPostProcessor ## start ros node ## self.fsm_debugpub = rospy.Publisher('fsm_debugpub', String, queue_size=10)
def run(self): while True: self.sensors.camera.update() if self.timer.millis() > 100: self.sensors.update() self.wallFollow("Right", self.WALL_FOLLOW_SPEED, self.wallFollowPID) # check timeout and block posession if self.timeout.isTimeUp() == True: print "Done with blind wall following." return startState.startState( self.sensors, self.actuators, self.motorController, self.timer, self.utils ) if self.sensors.uIR.val == 0: print "Break beam has sensed a block. Going to Pick Up Block State..." return pickUpBlockState.PickUpBlockState( self.sensors, self.actuators, self.motorController, self.timer, self.utils ) self.actuators.update() self.motorController.updateMotorSpeeds() self.timer.reset()
def run(self): while True: self.sensors.camera.update() if self.timer.millis() > 100: self.sensors.update() #constantly check to see if we have something to eat if self.sensors.uIR == 0: self.motorController.fwdVel = 0 return pickUpBlockState.PickUpBlockState(self.sensors, self.actuators, self.motorController, self.timer, self.utils) if self.substate == "ApproachBlock": if self.isColliding(): self.motorController.fwdVel = 0 isManueverPossible = self.calculateFlankManeuver() if isManueverPossible == True: self.turnNDegreesSlowly(self.flank_first_angle) self.flank_maneuver_attempts += 1 self.substate = "FlankManeuverTurn1" else: print 'Area too cramped to start Flank Maneuver. Perhaps we are in a corner? Being blind wall following.' return blindWallFollowingState.BlindWallFollowingState(self.sensors, self.actuators, self.motorController, self.timer) elif self.sensors.camera.blockDistance < self.CLOSE_ENOUGH_DISTANCE: print 'Finished approaching block. Will now try to eat it.' self.substate = "EatBlock" self.motorController.fwdVel = self.DRIVE_SPEED self.sensors.encoders.resetEncoders() elif abs(self.sensors.camera.blockAngle) > self.ANGLE_EPSILON: print 'Have turned too far from the direction of the block. Will reposition...' self.motorController.fwdVel = 0 return turnToBlockState.TurnToBlockState(self.sensors, self.actuators, self.motorController, self.timer, self.utils) elif self.substate == "EatBlock": if self.isColliding(): #To implement: if we got close enough, go to drag block state until we are free to move again. #Otherwise, see if you can't scan the area to make sure you're gonna crash if you keep going forward #if you do find you will crash, see if you can do a tight flank maneuver, or just give up #FORGET IT, don't do scan. Go straight to drag or flank maneuver. elif self.current_distance_traveled >= self.EAT_DISTANCE: print 'Finished attempt to eat block. Break beam did not go off.' return checkForMoreBlocksState.CheckForMoreBlocksState(self.sensors, self.actuators, self.motorController, self.timer) elif self.substate == "FlankManeuverTurn1": if self.isFinishedTurning() == True: self.motorController.fwdVel = self.DRIVE_SPEED self.substate = "FlankManeuverTravel" elif self.substate == "FlankManeuverTravel": if self.isColliding(): # keep turning until you find an opening or until you reach 90 degrees. If you reach 90 degrees, give up, you're cramped. elif self.current_distance_traveled >= self.flank_target_distance: self.motorController.fwdVel = 0 elif self.substate == "FlankManeuverTurn2": if self.isFinishedTurning() == True: self.substate == "ApproachBlock" else: Print 'Error! Substate named ', self.substate, ' was not recognized. Exiting to Start State...' return startState.startState(self.sensors, self.actuators, self.motorController, self.timer) self.actuators.update() self.motorController.updateMotorSpeeds() self.timer.reset()