def step(self, worldWrapper):
world = worldWrapper.world
# Do global checks
newState, changed = self.checkGlobal(worldWrapper)
if changed == STATE_CHANGE_FLAG:
return newState, changed
# Check for yellow wall
# Check for critical time
if self.checkCritical(worldWrapper):
return DriveToWallState(worldWrapper), STATE_CHANGE_FLAG
# Check for Balls
if len(world.balls) > 0:
print "Found some balls in drive straight state", world.balls
return BallAcquisitionState(worldWrapper), STATE_CHANGE_FLAG
# Set our goal
goal = DriveStraightState.GOAL
# With 0.50 chance per second, switch to turning.
# TODO: Check this and tweak it for optimal performance
if random.random() < timeEqualizedRandom(self.lastTime, worldWrapper.time, 0.20):
return TurnState(worldWrapper), STATE_CHANGE_FLAG
if random.random() < timeEqualizedRandom(self.lastTime, worldWrapper.time, 0.50):
return DriveToWallState(worldWrapper), STATE_CHANGE_FLAG
# Otherwise, full steam ahead.
else:
self.lastTime = worldWrapper.time
return self, goal
def step(self, worldWrapper):
# Check our global conditions
newState, changed = self.checkGlobal(worldWrapper)
if(changed == STATE_CHANGE_FLAG):
return newState, changed
# Check for balls
newState,changed = self.checkForBalls(worldWrapper)
if(changed == STATE_CHANGE_FLAG):
return newState, changed
# Check for yellow wall, and change states if the time is greater than 150
# Check for critical time
if self.checkCritical(worldWrapper):
return DriveToWallState(worldWrapper), STATE_CHANGE_FLAG
# Set the goal location
goal = TurnState.GOAL
# With 0.50 chance per second, switch to driving forward.
# TODO: Check this value and tweak for best performance
if random.random() < timeEqualizedRandom(self.lastTime, worldWrapper.time, 0.5):
return DriveStraightState(worldWrapper), STATE_CHANGE_FLAG
if random.random() < timeEqualizedRandom(self.lastTime, worldWrapper.time, 0.7):
return DriveToWallState(worldWrapper), STATE_CHANGE_FLAG
# Otherwise, turn.
else:
self.lastTime = worldWrapper.time
return self, goal
def step(self, worldWrapper):
world = worldWrapper.world
newState,changed = self.checkGlobal(worldWrapper,False)
if(changed == STATE_CHANGE_FLAG):
return newState, changed
newState,changed = self.checkForBalls(worldWrapper)
if(changed == STATE_CHANGE_FLAG):
return newState, changed
self.lastTime = worldWrapper.time
if(self.startTime - worldWrapper.time >= self.TIMEOUT):
return EscapeState(worldWrapper), STATE_CHANGE_FLAG
if(random.random() < timeEqualizedRandom(self.lastTime, worldWrapper.time, 0.20)):
return TurnState(worldWrapper), STATE_CHANGE_FLAG
if(world.bumpData != None and (world.bumpData.left or world.bumpData.right or world.bumpData.left)):
return AlignToWall(worldWrapper), STATE_CHANGE_FLAG
if(world.irData != None and world.irData.left < self.GOOD_DIST):
return FollowWallState(worldWrapper), STATE_CHANGE_FLAG
return self, self.GOAL
def step(self, worldWrapper):
# Check for deadness, bump sensors
newState, changed = self.checkGlobal(worldWrapper,False)
if (changed == STATE_CHANGE_FLAG):
return newState, changed
# Check for balls
newState,changed = self.checkForBalls(worldWrapper)
if(changed == STATE_CHANGE_FLAG):
return newState, changed
if(worldWrapper.world.bumpData != None and
(worldWrapper.world.bumpData.left or
worldWrapper.world.bumpData.right or
worldWrapper.world.bumpData.front)):
return AlignToWall(worldWrapper), STATE_CHANGE_FLAG
if not worldWrapper.world.irData == None:
irData = worldWrapper.world.irData
print irData.leftFront, irData.leftSide
'''
if(random.random() < timeEqualizedRandom(self.lastTime, worldWrapper.time, 0.10)):
return TurnState(worldWrapper), STATE_CHANGE_FLAG
'''
# Determine which direction we're turned in
if (irData.leftFront > irData.leftSide + self.TURNED_THRESH_RIGHT):
print "We're turned right"
turned = self.TURNED_RIGHT
elif (irData.leftSide > irData.leftFront + self.TURNED_THRESH_LEFT):
print "We're turned left"
turned = self.TURNED_LEFT
else:
print "We're turned straight"
turned = self.TURNED_STRAIGHT
if random.random() < timeEqualizedRandom(self.lastTime, worldWrapper.time, 0.1):
return DriveToWallState(worldWrapper), STATE_CHANGE_FLAG
#BLOCK OF LOGIC
if(turned == self.TURNED_RIGHT):
if(irData.leftSide <= self.WALL_FOLLOW_CLOSE):
print "Turned right, too close"
return self, self.TURNING_GOAL_L
else:
print "Turned right, just fine"
return self, self.STRAIGHT_GOAL
elif(turned == self.TURNED_LEFT):
if(irData.leftFront >= self.WALL_FOLLOW_FAR):
print "Turned left, too far"
return self, self.TURNING_GOAL_R
else:
print "Turned left, just fine"
return self, self.STRAIGHT_GOAL
else:
if(irData.leftSide <= self.WALL_FOLLOW_CLOSE or irData.leftFront <= self.WALL_FOLLOW_CLOSE):
print "Straight, too close"
return self, self.TURNING_GOAL_R
elif(irData.leftSide >= self.WALL_FOLLOW_FAR or irData.leftFront >= self.WALL_FOLLOW_FAR):
print "Straight, too far"
return self, self.TURNING_GOAL_L
else:
print "Straight, just right"
return self, self.STRAIGHT_GOAL
"""