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