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()