def run(self): while True: self.sensors.camera.update() if self.timer.millis() > 100: self.sensors.update() if self.timeout.isTimeUp() == True: print 'State Timeout has timed out. Going to Random Travel State...' return randomTravelingState.RandomTravelingState(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.sensors.camera.detectBlock: print "Found Block" return turnToBlockState.TurnToBlockState(self.sensors, self.actuators, self.motorController, self.timer, self.utils) elif self.sensors.gyro.gyroCAngle>self.initialAngle+360: return wallFollowingState.WallFollowingState(self.sensors, self.actuators, self.motorController, self.timer, self.utils) else: self.turnConstantRate(self.SCAN_SPEED,"Right") 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()
#check timeout and block posession
if self.timeout.isTimeUp() == True:
print 'StrictWallFollowingState complete. Did not find the block again...'
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.wallFollow("Right")
if self.sensors.camera.detectBlock and self.sensors.camera.blockDistance < self.STRICT_BLOCK_DISTANCE_THRESHOLD:
return turnToBlockState.TurnToBlockState(
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.sensors.button.val == 1 and self.utils.isGameStarted == False:
self.utils.isGameStarted = True
self.utils.startTime = time.time()
if (self.utils.isGameStarted == True
and time.time() - self.utils.startTime >
self.START_GAME_DELAY):
#return testState.TestState(self.sensors,self.actuators,self.motorController,self.timer, self.utils)
if self.sensors.camera.detectBlock:
return turnToBlockState.TurnToBlockState(
self.sensors, self.actuators, self.motorController,
self.timer, self.utils)
else:
self.utils.navTimeout.reset()
return lookingForBlocksState.LookingForBlocksState(
self.sensors, self.actuators, self.motorController,
self.timer, self.utils)
#return randomTravelingState.RandomTravelingState(self.sensors,self.actuators,self.motorController,self.timer, self.utils)
#return breakFreeState.BreakFreeState(self.sensors,self.actuators,self.motorController,self.timer, self.utils)
#return wallFollowingState.WallFollowingState(self.sensors,self.actuators,self.motorController,self.timer, self.utils)
#return blindWallFollowingState.BlindWallFollowingState(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()
#check timeout and block posession
if self.utils.navTimeout.isTimeUp() == True:
print 'Navigation Timeout timed out. Going to Random Traveling State...'
return randomTravelingState.RandomTravelingState(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.wallFollow("Right",self.WALL_FOLLOW_SPEED,self.wallFollowPID)
if self.sensors.camera.detectBlock:
return turnToBlockState.TurnToBlockState(self.sensors, self.actuators, self.motorController, self.timer, self.utils)
elif (time.time()-self.startStateTime)>=self.WALL_FOLLOW_TIME:
return lookingForBlocksState.LookingForBlocksState(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.sensors.button==1 and self.isGameStarted==False: self.isGameStarted=True self.utils.startTime=time.time() if(self.utils.isGameStarted==True and time.time()-self.utils.startTime>self.START_GAME_DELAY): if self.sensors.camera.detectBlock: return turnToBlockState.TurnToBlockState(self.sensors,self.actuators,self.motorController,self.timer, self.utils)) else: return lookingForBlocksState.LookingForBlocksState(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.sensors.camera.detectBlock:
return turnToBlockState.TurnToBlockState(
self.sensors, self.actuators, self.motorController,
self.timer, self.utils)
elif self.sensors.gyro.gyroCAngle > self.initialAngle + 360:
return wallFollowingState.WallFollowingState(
self.sensors, self.actuators, self.motorController,
self.timer, self.utils)
else:
self.turnConstantRate(self.SCAN_SPEED)
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")
if self.sensors.camera.detectBlock:
return turnToBlockState.TurnToBlockState(
self.sensors, self.actuators, self.motorController,
self.timer, self.utils)
elif (time.time() -
self.startStateTime) >= self.WALL_FOLLOW_TIME:
return lookingForBlocksState.LookingForBlocksState(
self.sensors, self.actuators, self.motorController,
self.timer, self.utils)
else:
self.turnConstantRate(self.SCAN_SPEED)
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()
def run(self):
while True:
self.sensors.camera.update()
if self.timer.millis() > 100:
self.sensors.update()
if self.timeout.isTimeUp() == True:
print 'State Timeout has timed out. Going to StartState...'
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)
if self.substate == "RandomTraveling":
angleToTurn = self.checkCollisionAngle()
if angleToTurn is not None:
print 'We have collided. Bouncing off a random angle, ', angleToTurn, ' degrees...'
if angleToTurn < 0:
self.lastTurnNegative = True
else:
self.lastTurnNegative = False
self.driveStraight(0)
self.turnNDegreesSlowly(
random.randrange(
angleToTurn - self.RANDOM_ANGLE_RANGE,
angleToTurn + self.RANDOM_ANGLE_RANGE))
self.substate = "Turning"
else:
self.turnConstantRate(0, "Right")
self.driveStraight(self.DRIVE_SPEED)
if self.sensors.camera.detectBlock == True:
print 'Block Detected. Turning to it...'
return turnToBlockState.TurnToBlockState(
self.sensors, self.actuators,
self.motorController, self.timer, self.utils)
elif self.substate == "Turning":
if self.hasFinishedTurning():
if self.isStillColliding():
print 'After turn, we are still colliding. Will start scanning for an open spot...'
self.substate = "Scanning"
self.intialGyroAngle = self.sensors.gyro.gyroCAngle
if self.lastTurnNegative:
self.tturnConstantRate(self.TURN_SPEED,
"Right")
else:
self.turnConstantRate(self.TURN_SPEED, "Left")
else:
print 'Not colliding anymore...'
self.turnConstantRate(0, "Right")
self.driveStraight(self.DRIVE_SPEED)
self.substate = "RandomTraveling"
elif self.substate == "Scanning":
if self.isStillColliding() == False:
print 'Finally found an open spot...'
self.driveStraight(self.DRIVE_SPEED)
self.turnConstantRate(0, "Right")
self.substate = "RandomTraveling"
elif (self.lastTurnNegative) and (
self.sensors.gyro.gyroCAngle <
(self.intialGyroAngle - 360)):
print 'Not good. After full rotation, we cannot find an open spot. I guess we can go to start state and see what wall following can do? (We made a full rotation, so we are not jammed.)'
return startState.StartState(self.sensors,
self.actuators,
self.motorController,
self.timer, self.utils)
elif (self.lastTurnNegative
== False) and (self.sensors.gyro.gyroCAngle >
(self.intialGyroAngle + 360)):
print 'Not good. After full rotation, we cannot find an open spot. I guess we can go to start state and see what wall following can do? (We made a full rotation, so we are not jammed.)'
return startState.StartState(self.sensors,
self.actuators,
self.motorController,
self.timer, self.utils)
else:
print 'Error! Substate ', self.substate, ' not a valid substate. Returning to StartState...'
return startState.StartState(self.sensors, self.actuators,
self.motorController,
self.timer, self.utils)
self.actuators.update()
self.motorController.updateMotorSpeeds()
self.timer.reset()