class RoPiTrack: # initialize def __init__(self, remote_address): # set PINs on BOARD log.debug("Initializing Track...") log.debug("> track 1: " + str(_conf['track_1_pin'])) log.debug("> track 2: " + str(_conf['track_2_pin'])) # using LineSensor rem_pi = PiGPIOFactory(host=remote_address) self.track1 = LineSensor(_conf['track_1_pin'], pin_factory=rem_pi) self.track2 = LineSensor(_conf['track_2_pin'], pin_factory=rem_pi) log.debug("...init done!") # are tracks on line? def is_on_line(self): track1 = self.track1.value == _conf['color'] track2 = self.track2.value == _conf['color'] log.debug("> is on line ?" + str(track1 and track2)) return track1 and track2 # is track1 on line? def track1_on_line(self): track1 = self.track1.value == _conf['color'] log.debug("> is track 1 on line?" + str(track1)) return track1 # is track2 on line? def track2_on_line(self): track2 = self.track2.value == _conf['color'] log.debug("> is track 2 on line?" + str(track2)) return track2 # terminate def terminate(self): log.debug("Track termination...") self.track1.close() self.track2.close()
#LineSensor test from gpiozero import LineSensor from time import sleep from signal import pause def lineDetected(): print('line detected') def noLineDetected(): print('no line detected') sensor = LineSensor(14) sensor.when_line = lineDetected sensor.when_no_line = noLineDetected pause() sensor.close()
class LineNavigator: # Time interval between subsequent sensor readings. _FRAME_RATE_s = 100e-6 # 100 us class _State(enum.Enum): LEFT_ON_TRACK = 0 RIGHT_ON_TRACK = 1 BOTH_ON_TRACK = 2 NONE_ON_TRACK = 3 def __init__(self, driver, status_led, black_track=True): self._driver = driver self._status_led = status_led self._black_track = black_track self._sensor_left = LineSensor(pin=PIN_LEFT_LINE_SENSOR, pull_up=lts.IS_PULL_UP) self._sensor_right = LineSensor(pin=PIN_RIGHT_LINE_SENSOR, pull_up=lts.IS_PULL_UP) # These callback functions are called the first time that one of the # relative events occurs. self._callbacks = { self._State.BOTH_ON_TRACK: self._both_on_track_callback, self._State.LEFT_ON_TRACK: self._left_on_track_callback, self._State.RIGHT_ON_TRACK: self._right_on_track_callback, self._State.NONE_ON_TRACK: self._none_on_track_callback, } # Assume the robot is well-centered on the track. self._state = self._State.NONE_ON_TRACK self._status_led.set(ls.Status.AUTOPILOT) _logger.info('{} initialized'.format(self.__class__.__name__)) def _none_on_track_callback(self): # Reset forward direction. self._driver.set_command(command_code=dvr.COMMAND_RIGHT, command_value=False) self._driver.set_command(command_code=dvr.COMMAND_LEFT, command_value=False) self._driver.set_command(command_code=dvr.COMMAND_FORWARD, command_value=True) _logger.debug('No line detected: go straight ahead') def _left_on_track_callback(self): # Sharp turn to left. self._driver.set_command(command_code=dvr.COMMAND_FORWARD, command_value=False) self._driver.set_command(command_code=dvr.COMMAND_LEFT, command_value=True) _logger.debug('Adjust left') def _right_on_track_callback(self): # Sharp turn to right. self._driver.set_command(command_code=dvr.COMMAND_FORWARD, command_value=False) self._driver.set_command(command_code=dvr.COMMAND_RIGHT, command_value=True) _logger.debug('Adjust right') def _both_on_track_callback(self): self._driver.stop() _logger.warning('Both line sensors detected a line: stop the motors') def run(self): # Start the robot. self._driver.set_command(command_code=dvr.COMMAND_FORWARD, command_value=True) while True: # Active sensor means that the track was detected. if self._black_track: left = not self._sensor_left.is_active right = not self._sensor_right.is_active else: left = self._sensor_left.is_active right = self._sensor_right.is_active if left and right: self._state = self._State.BOTH_ON_TRACK elif left and not right: self._state = self._State.LEFT_ON_TRACK elif not left and right: self._state = self._State.RIGHT_ON_TRACK else: self._state = self._State.NONE_ON_TRACK callback = self._callbacks[self._state] if callback is not None: callback() time.sleep(self._FRAME_RATE_s) def close(self): self._driver.stop() self._sensor_left.close() self._sensor_right.close() _logger.info('{} stopped'.format(self.__class__.__name__))
l_sensor = LineSensor(LS) r_sensor = LineSensor(RS) speed = 0.68 def motor_speed(): while True: l_detect = int(l_sensor.value) r_detect = int(r_sensor.value) ## Stage 1 if l_detect == 0 and r_detect == 0: l_motor = 1 r_motor = 1 ## Stage 2 if l_detect == 0 and r_detect == 1: l_motor = -1 if l_detect == 1 and r_detect == 0: r_motor = -1 #print(r, l) yield (r_motor * speed, l_motor * speed) robot.source = motor_speed() sleep(60) robot.stop() robot.source = None robot.close() l_sensor.close() r_sensor.close()
def motor_speed(): while True: left_detect = int(left_sensor.value) right_detect = int(right_sensor.value) front_detect = int(front_sensor.value) left_mot = 0 right_mot = 0 if left_detect == 0 and right_detect == 1: left_mot = 1 elif left_detect == 1 and right_detect == 0: right_mot = -1 elif front_detect == 0: right_mot, left_mot = 0, 0 else: left_mot = -1 right_mot = 0.9 yield right_mot, left_mot robot.source = motor_speed() sleep(60) robot.stop() robot.source = None robot.close() left_sensor.close() right_sensor.close() front_sensor.close()
from gpiozero import Robot, LineSensor from time import sleep from signal import pause #import RPi.GPIO as GPIO left_sensor = LineSensor(pin=21, pull_up=None, active_state=True) #right_sensor = LineSensor(20) #GPIO.setmode(GPIO.BOARD) #GPIO.setup(21, GPIO.IN) try: while True: if left_sensor.when_no_line: print('White') else: print("Black") except: left_sensor.close()