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 stop(self):
if self._timer:
self._timer.cancel()
if self.configurator.drive:
self.configurator.drive.stop()
ev3.unregisterEvent(self.configurator.touch_sensor.is_pressed)
super(ModifiedEV3RStormRobotDriver_1, self).stop()
def _unregister_from_keys_no_recovery(self):
# TODO this deserves a loop.
ev3.unregisterEvent(ev3.is_enter_button_pressed)
ev3.unregisterEvent(ev3.is_up_button_pressed)
ev3.unregisterEvent(ev3.is_down_button_pressed)
ev3.unregisterEvent(ev3.is_left_button_pressed)
ev3.unregisterEvent(ev3.is_right_button_pressed)
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 _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 stop(self):
if self.configurator.drive:
self.configurator.drive.stop()
ev3.unregisterEvent(self.configurator.touch_sensor.is_pressed)
ev3.unregisterEvent(self._is_close_to_object)
super(ModifiedEV3RStormRobotDriver_1, self).stop()
def stop(self):
if self.configurator.drive:
self.configurator.drive.stop()
ev3.unregisterEvent(self.configurator.touch_sensor.is_pressed)
ev3.unregisterEvent(self._is_close_to_object)
super(ModifiedEV3RStormRobotDriver_1, self).stop()
