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()
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()
#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()
#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()
#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)
if self.sensors.camera.detectBlock == False:
print "Lost sight of block while turning!"
return lookingForBlocksState.LookingForBlocksState(
self.sensors, self.actuators, self.motorController,
self.timer, self.utils)
elif self.sensors.camera.detectBlock == True:
if abs(self.sensors.camera.blockAngle
) < self.BLOCK_ANGLE_EPSILON:
print "MOVE TO BLOCK"
return moveToBlockState.MoveToBlockState(
self.sensors, self.actuators, self.motorController,
self.timer, self.utils)
else:
print "turning to block"
print self.sensors.camera.blockAngle
#print self.motorController.motorState
self.turnNDegreesSlowly(self.sensors.camera.blockAngle)
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()