def _next_step(self, because_is_close_to_object=False):
if because_is_close_to_object:
# Unregister to avoid spam until the object is avoided.
ev3.unregisterEvent(self._is_close_to_object)
else:
# Might have been unregistered so make sure it's always registered.
ev3.registerEvent(self._is_close_to_object, self._next_step, True)
if self._timer:
self._timer.cancel()
self.configurator.drive.stop()
# print "Close to object {0}".format(because_is_close_to_object)
if len(self._path) > 0:
moves = self._path[0]
del self._path[0]
move = moves[0]
time = moves[1]
# print "Move -> time {0} {1}".format(move, time)
# Skip going forward if there's some obstackle in the way.
self._timer = Timer(time if not because_is_close_to_object or move != self._DIR_STRAIGHT else 0,
self._next_step)
self._timer.start()
if move == self._DIR_STRAIGHT:
if not because_is_close_to_object:
self.configurator.drive.start(EV3Drive.MOVE_FORWARD, self.speed)
else:
ev3.motor.lego.turn(self.configurator.drive,
EV3Drive.TURN_LEFT if move == self._DIR_LEFT else EV3Drive.TURN_RIGHT,
self.speed,
self.speed,
self.configurator.get_left_motors(),
self.configurator.get_right_motors(),
True)
else:
self.start()
def _change_direction(self):
# Check right side.
ev3.unregisterEvent(self._is_close_to_object)
ev3.set_led(ev3.LED_RED_PULSE)
ev3.motor.lego.turn(self.configurator.drive, EV3Drive.TURN_RIGHT,
self.speed, self.speed,
self.configurator.get_left_motors(),
self.configurator.get_right_motors(), True)
time.sleep(self.turn_time)
self.configurator.drive.stop()
if self._is_close_to_object():
# Right side is not good, turn left
ev3.motor.lego.turn(self.configurator.drive, EV3Drive.TURN_LEFT,
self.speed, self.speed,
self.configurator.get_left_motors(),
self.configurator.get_right_motors(), True)
time.sleep(2 * self.turn_time)
self.configurator.drive.stop()
if self._is_close_to_object():
# Left is not good, go back.
ev3.motor.lego.turn(self.configurator.drive, EV3Drive.TURN_LEFT,
self.speed, self.speed,
self.configurator.get_left_motors(),
self.configurator.get_right_motors(), True)
time.sleep(self.turn_time)
self.configurator.drive.stop()
self.configurator.drive.start(EV3Motor.MOVE_FORWARD, self.speed)
ev3.set_led(ev3.LED_GREEN_PULSE)
ev3.registerEvent(self._is_close_to_object, self._change_direction)
def _unregister_from_keys(self):
# TODO this deserves a loop.
ev3.unregisterEvent(ev3.is_enter_button_pressed)
if self._old_handler[self._KEY_ENTER]:
ev3.registerEvent(ev3.is_enter_button_pressed,
self._old_handler[self._KEY_ENTER]['handle'],
*(self._old_handler[self._KEY_ENTER]['args']))
ev3.unregisterEvent(ev3.is_up_button_pressed)
if self._old_handler[self._KEY_UP]:
ev3.registerEvent(ev3.is_up_button_pressed,
self._old_handler[self._KEY_UP]['handle'],
*(self._old_handler[self._KEY_UP]['args']))
ev3.unregisterEvent(ev3.is_down_button_pressed)
if self._old_handler[self._KEY_DOWN]:
ev3.registerEvent(ev3.is_down_button_pressed,
self._old_handler[self._KEY_DOWN]['handle'],
*(self._old_handler[self._KEY_DOWN]['args']))
ev3.unregisterEvent(ev3.is_left_button_pressed)
if self._old_handler[self._KEY_LEFT]:
ev3.registerEvent(ev3.is_left_button_pressed,
self._old_handler[self._KEY_LEFT]['handle']
*(self._old_handler[self._KEY_LEFT]['args']))
ev3.unregisterEvent(ev3.is_right_button_pressed)
if self._old_handler[self._KEY_RIGHT]:
ev3.registerEvent(ev3.is_right_button_pressed,
self._old_handler[self._KEY_RIGHT]['handle']
*(self._old_handler[self.KEY_RIGHT]['args']))
def _unregister_from_keys(self):
# TODO this deserves a loop.
ev3.unregisterEvent(ev3.is_enter_button_pressed)
if self._old_handler[self._KEY_ENTER]:
ev3.registerEvent(ev3.is_enter_button_pressed,
self._old_handler[self._KEY_ENTER]['handle'],
*(self._old_handler[self._KEY_ENTER]['args']))
ev3.unregisterEvent(ev3.is_up_button_pressed)
if self._old_handler[self._KEY_UP]:
ev3.registerEvent(ev3.is_up_button_pressed,
self._old_handler[self._KEY_UP]['handle'],
*(self._old_handler[self._KEY_UP]['args']))
ev3.unregisterEvent(ev3.is_down_button_pressed)
if self._old_handler[self._KEY_DOWN]:
ev3.registerEvent(ev3.is_down_button_pressed,
self._old_handler[self._KEY_DOWN]['handle'],
*(self._old_handler[self._KEY_DOWN]['args']))
ev3.unregisterEvent(ev3.is_left_button_pressed)
if self._old_handler[self._KEY_LEFT]:
ev3.registerEvent(
ev3.is_left_button_pressed,
self._old_handler[self._KEY_LEFT]['handle'] *
(self._old_handler[self._KEY_LEFT]['args']))
ev3.unregisterEvent(ev3.is_right_button_pressed)
if self._old_handler[self._KEY_RIGHT]:
ev3.registerEvent(
ev3.is_right_button_pressed,
self._old_handler[self._KEY_RIGHT]['handle'] *
(self._old_handler[self.KEY_RIGHT]['args']))
def _next_step(self, because_is_close_to_object=False):
if because_is_close_to_object:
# Unregister to avoid spam until the object is avoided.
ev3.unregisterEvent(self._is_close_to_object)
else:
# Might have been unregistered so make sure it's always registered.
ev3.registerEvent(self._is_close_to_object, self._next_step, True)
if self._timer:
self._timer.cancel()
self.configurator.drive.stop()
# print "Close to object {0}".format(because_is_close_to_object)
if len(self._path) > 0:
moves = self._path[0]
del self._path[0]
move = moves[0]
time = moves[1]
# print "Move -> time {0} {1}".format(move, time)
# Skip going forward if there's some obstackle in the way.
self._timer = Timer(
time if not because_is_close_to_object
or move != self._DIR_STRAIGHT else 0, self._next_step)
self._timer.start()
if move == self._DIR_STRAIGHT:
if not because_is_close_to_object:
self.configurator.drive.start(EV3Drive.MOVE_FORWARD,
self.speed)
else:
ev3.motor.lego.turn(
self.configurator.drive, EV3Drive.TURN_LEFT if move
== self._DIR_LEFT else EV3Drive.TURN_RIGHT, self.speed,
self.speed, self.configurator.get_left_motors(),
self.configurator.get_right_motors(), True)
else:
self.start()
def _register_for_keys(self):
self._old_handler[self._KEY_ENTER] = ev3.registerEvent(ev3.is_enter_button_pressed,
self._enter_key_pressed)
self._old_handler[self._KEY_UP] = ev3.registerEvent(ev3.is_up_button_pressed,
self._up_pressed)
self._old_handler[self._KEY_DOWN] = ev3.registerEvent(ev3.is_down_button_pressed,
self._down_pressed)
self._old_handler[self._KEY_LEFT] = ev3.registerEvent(ev3.is_left_button_pressed,
self._left_pressed)
self._old_handler[self._KEY_RIGHT] = ev3.registerEvent(ev3.is_right_button_pressed,
self._right_pressed)
def start(self):
print "Starting 1"
super(ModifiedEV3RStormRobotDriver_1, self).start()
if not self.configurator.drive:
print "No drive no fun. Quitting."
return
# Go forward with 1/4 speed
self.configurator.drive.start(EV3Motor.MOVE_FORWARD, self.speed)
ev3.registerEvent(self.configurator.touch_sensor.is_pressed, self.stop)
ev3.registerEvent(self._is_close_to_object, self._change_direction)
def _register_for_keys(self):
self._old_handler[self._KEY_ENTER] = ev3.registerEvent(
ev3.is_enter_button_pressed, self._enter_key_pressed)
self._old_handler[self._KEY_UP] = ev3.registerEvent(
ev3.is_up_button_pressed, self._up_pressed)
self._old_handler[self._KEY_DOWN] = ev3.registerEvent(
ev3.is_down_button_pressed, self._down_pressed)
self._old_handler[self._KEY_LEFT] = ev3.registerEvent(
ev3.is_left_button_pressed, self._left_pressed)
self._old_handler[self._KEY_RIGHT] = ev3.registerEvent(
ev3.is_right_button_pressed, self._right_pressed)
def start(self):
print "Starting 1"
super(ModifiedEV3RStormRobotDriver_1, self).start()
if not self.configurator.drive:
print "No drive no fun. Quitting."
return
# Go forward with 1/4 speed
self.configurator.drive.start(EV3Motor.MOVE_FORWARD, self.speed)
ev3.registerEvent(self.configurator.touch_sensor.is_pressed, self.stop)
ev3.registerEvent(self._is_close_to_object, self._change_direction)
def _change_direction(self):
# Check right side.
ev3.unregisterEvent(self._is_close_to_object)
ev3.set_led(ev3.LED_RED_PULSE)
ev3.motor.lego.turn(self.configurator.drive,
EV3Drive.TURN_RIGHT,
self.speed,
self.speed,
self.configurator.get_left_motors(),
self.configurator.get_right_motors(),
True)
time.sleep(self.turn_time)
self.configurator.drive.stop()
distance_right = self.configurator.ir_sensor.get_distance()
if distance_right <= 73:
# There is some object in sight. Check left.
ev3.motor.lego.turn(self.configurator.drive,
EV3Drive.TURN_LEFT,
self.speed,
self.speed,
self.configurator.get_left_motors(),
self.configurator.get_right_motors(),
True)
time.sleep(2 * self.turn_time)
self.configurator.drive.stop()
distance_left = self.configurator.ir_sensor.get_distance()
if distance_left <= self.close_to_object_ir_threshold and distance_right <= self.close_to_object_ir_threshold:
# Left is not good. Same right. Go back.
ev3.motor.lego.turn(self.configurator.drive,
EV3Drive.TURN_LEFT,
self.speed,
self.speed,
self.configurator.get_left_motors(),
self.configurator.get_right_motors(),
True)
time.sleep(self.turn_time)
self.configurator.drive.stop()
else:
if distance_right >= distance_left: # Go right
ev3.motor.lego.turn(self.configurator.drive,
EV3Drive.TURN_RIGHT,
self.speed,
self.speed,
self.configurator.get_left_motors(),
self.configurator.get_right_motors(),
True)
time.sleep(2 * self.turn_time)
# else go left
self.configurator.drive.start(EV3Motor.MOVE_FORWARD, self.speed)
ev3.set_led(ev3.LED_GREEN_PULSE)
ev3.registerEvent(self._is_close_to_object, self._change_direction)
def _change_direction(self):
# Check right side.
ev3.unregisterEvent(self._is_close_to_object)
ev3.set_led(ev3.LED_RED_PULSE)
ev3.motor.lego.turn(self.configurator.drive, EV3Drive.TURN_RIGHT,
self.speed, self.speed,
self.configurator.get_left_motors(),
self.configurator.get_right_motors(), True)
time.sleep(self.turn_time)
self.configurator.drive.stop()
distance_right = self.configurator.ir_sensor.get_distance()
if distance_right <= 73:
# There is some object in sight. Check left.
ev3.motor.lego.turn(self.configurator.drive, EV3Drive.TURN_LEFT,
self.speed, self.speed,
self.configurator.get_left_motors(),
self.configurator.get_right_motors(), True)
time.sleep(2 * self.turn_time)
self.configurator.drive.stop()
distance_left = self.configurator.ir_sensor.get_distance()
if distance_left <= self.close_to_object_ir_threshold and distance_right <= self.close_to_object_ir_threshold:
# Left is not good. Same right. Go back.
ev3.motor.lego.turn(self.configurator.drive,
EV3Drive.TURN_LEFT, self.speed, self.speed,
self.configurator.get_left_motors(),
self.configurator.get_right_motors(), True)
time.sleep(self.turn_time)
self.configurator.drive.stop()
else:
if distance_right >= distance_left: # Go right
ev3.motor.lego.turn(self.configurator.drive,
EV3Drive.TURN_RIGHT, self.speed,
self.speed,
self.configurator.get_left_motors(),
self.configurator.get_right_motors(),
True)
time.sleep(2 * self.turn_time)
# else go left
self.configurator.drive.start(EV3Motor.MOVE_FORWARD, self.speed)
ev3.set_led(ev3.LED_GREEN_PULSE)
ev3.registerEvent(self._is_close_to_object, self._change_direction)
def _change_direction(self):
# Check right side.
ev3.unregisterEvent(self._is_close_to_object)
ev3.set_led(ev3.LED_RED_PULSE)
ev3.motor.lego.turn(self.configurator.drive,
EV3Drive.TURN_RIGHT,
self.speed,
self.speed,
self.configurator.get_left_motors(),
self.configurator.get_right_motors(),
True)
time.sleep(self.turn_time)
self.configurator.drive.stop()
if self._is_close_to_object():
# Right side is not good, turn left
ev3.motor.lego.turn(self.configurator.drive,
EV3Drive.TURN_LEFT,
self.speed,
self.speed,
self.configurator.get_left_motors(),
self.configurator.get_right_motors(),
True)
time.sleep(2 * self.turn_time)
self.configurator.drive.stop()
if self._is_close_to_object():
# Left is not good, go back.
ev3.motor.lego.turn(self.configurator.drive,
EV3Drive.TURN_LEFT,
self.speed,
self.speed,
self.configurator.get_left_motors(),
self.configurator.get_right_motors(),
True)
time.sleep(self.turn_time)
self.configurator.drive.stop()
self.configurator.drive.start(EV3Motor.MOVE_FORWARD, self.speed)
ev3.set_led(ev3.LED_GREEN_PULSE)
ev3.registerEvent(self._is_close_to_object, self._change_direction)
def _go(self):
ev3.set_led(ev3.LED_GREEN)
ev3.registerEvent(self.configurator.touch_sensor.is_pressed, self.stop)
ev3.registerEvent(self._is_close_to_object, self._next_step, True)
self._next_step()
def _register_for_keys_no_recovery(self):
ev3.registerEvent(ev3.is_enter_button_pressed, self._enter_key_pressed)
ev3.registerEvent(ev3.is_up_button_pressed, self._up_pressed)
ev3.registerEvent(ev3.is_down_button_pressed, self._down_pressed)
ev3.registerEvent(ev3.is_left_button_pressed, self._left_pressed)
ev3.registerEvent(ev3.is_right_button_pressed, self._right_pressed)
def _go(self):
ev3.set_led(ev3.LED_GREEN)
ev3.registerEvent(self.configurator.touch_sensor.is_pressed, self.stop)
ev3.registerEvent(self._is_close_to_object, self._next_step, True)
self._next_step()
def _register_for_keys_no_recovery(self):
ev3.registerEvent(ev3.is_enter_button_pressed, self._enter_key_pressed)
ev3.registerEvent(ev3.is_up_button_pressed, self._up_pressed)
ev3.registerEvent(ev3.is_down_button_pressed, self._down_pressed)
ev3.registerEvent(ev3.is_left_button_pressed, self._left_pressed)
ev3.registerEvent(ev3.is_right_button_pressed, self._right_pressed)
def get_driver(config):
if len(sys.argv) < 2 or sys.argv[1] == "1":
print "Variant 1"
driver = ModifiedEV3RStormRobotDriver_1(config)
elif sys.argv[1] == "2":
print "Variant 2"
driver = ModifiedEV3RStormRobotDriver_2(config)
else:
print "Variant 3"
driver = ModifiedEV3RStormRobotDriver_3(config)
return driver
if __name__ == "__main__":
ev3.open_all_devices()
robot = EV3Robot()
robot.setup(get_driver(ModifiedEV3RStormRobotConfig()))
ev3.registerEvent(ev3.is_enter_button_pressed, robot.start)
print "Ready..."
ev3.run()
print "Done..."
ev3.set_led(ev3.LED_BLACK)
ev3.close_all_devices()
def get_driver(config):
if len(sys.argv) < 2 or sys.argv[1] == "1":
print "Variant 1"
driver = ModifiedEV3RStormRobotDriver_1(config)
elif sys.argv[1] == "2":
print "Variant 2"
driver = ModifiedEV3RStormRobotDriver_2(config)
else:
print "Variant 3"
driver = ModifiedEV3RStormRobotDriver_3(config)
return driver
if __name__ == "__main__":
ev3.open_all_devices()
robot = EV3Robot()
robot.setup(get_driver(ModifiedEV3RStormRobotConfig()))
ev3.registerEvent(ev3.is_enter_button_pressed, robot.start)
print "Ready..."
ev3.run()
print "Done..."
ev3.set_led(ev3.LED_BLACK)
ev3.close_all_devices()
