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