robot.drive(0.10)
markov.move()
# rotate
direction = Orientation.calculate_direction((location[1], location[2]), destination)
while direction != markov._orientation:
rotate_direction = Orientation.rotate(markov._orientation, direction)
if rotate_direction == Orientation.LEFT:
markov.rotate_left()
robot.turn_left()
time.sleep(3)
elif rotate_direction == Orientation.RIGHT:
markov.rotate_right()
robot.turn_right()
time.sleep(3)
else:
pass
# calculate
time.sleep(2)
measurement = robot.sense_color()
markov.update(measurement)
if measurement == Colors.YELLOW:
if robot.sense_color() == Colors.YELLOW:
found = True
print "found"
print markov
time.sleep(1)
