Implemented auto pilot with distance sensors

This commit is contained in:
Fabio Manganiello 2018-02-26 11:31:06 +01:00
parent a8254181b2
commit cf83f07f79
3 changed files with 141 additions and 1 deletions

View file

@ -0,0 +1,99 @@
import logging
import threading
import time
import RPi.GPIO as gpio
from platypush.message.response import Response
from platypush.plugins import Plugin
class GpioSensorDistancePlugin(Plugin):
def __init__(self, trigger_pin, echo_pin):
self.trigger_pin = trigger_pin
self.echo_pin = echo_pin
self.measured_distance_mm = None
self._is_probing = False
gpio.setmode(gpio.BCM)
gpio.setup(self.trigger_pin, gpio.OUT)
gpio.setup(self.echo_pin, gpio.IN)
logging.info('Resetting trigger sensor on GPIO pin {}'.format(self.trigger_pin))
gpio.output(self.trigger_pin, False)
time.sleep(1)
def start(self):
self._is_probing = True
logging.info('Received distance sensor start command')
def _run():
gpio.setmode(gpio.BCM)
gpio.setup(self.trigger_pin, gpio.OUT)
gpio.setup(self.echo_pin, gpio.IN)
while self._is_probing:
gpio.output(self.trigger_pin, True)
time.sleep(0.00001) # 1 us pulse to trigger echo measurement
gpio.output(self.trigger_pin, False)
read_timeout = False
pulse_start = time.time()
pulse_on = pulse_start
while gpio.input(self.echo_pin) == 0:
pulse_on = time.time()
if pulse_on - pulse_start > 0.5:
logging.warning('Distance sensor echo timeout: 0')
read_timeout = True
break
pulse_start = pulse_on
pulse_end = time.time()
pulse_off = pulse_end
while gpio.input(self.echo_pin) == 1:
pulse_off = time.time()
if pulse_off - pulse_end > 0.5:
logging.warning('Distance sensor echo timeout: 1')
read_timeout = True
break
if read_timeout:
continue
pulse_end = pulse_off
pulse_duration = pulse_end - pulse_start
# s = vt where v = avg speed of sound
self.measured_distance_mm = round(pulse_duration * 171500.0, 2)
gpio.output(self.trigger_pin, False)
time.sleep(0.1)
gpio.cleanup()
self.measured_distance_mm = None
self._probe_thread = threading.Thread(target=_run)
self._probe_thread.start()
return Response(output={'status': 'probing'})
def get_distance(self):
return self.measured_distance_mm
def stop(self):
self._is_probing = False
self.measured_distance_mm = None
if self._probe_thread and threading.get_ident() != self._probe_thread.ident:
self._probe_thread.join()
return Response(output={'status':'stopped'})
# vim:sw=4:ts=4:et:

View file

@ -5,6 +5,8 @@ import time
from platypush.message.response import Response from platypush.message.response import Response
from platypush.plugins import Plugin from platypush.plugins import Plugin
from platypush.context import get_plugin
from platypush.config import Config
class Direction(enum.Enum): class Direction(enum.Enum):
@ -12,6 +14,7 @@ class Direction(enum.Enum):
DIR_DOWN = 'down' DIR_DOWN = 'down'
DIR_LEFT = 'left' DIR_LEFT = 'left'
DIR_RIGHT = 'right' DIR_RIGHT = 'right'
DIR_AUTO = 'auto'
class GpioZeroborgPlugin(Plugin): class GpioZeroborgPlugin(Plugin):
@ -37,6 +40,8 @@ class GpioZeroborgPlugin(Plugin):
self.v_in = v_in self.v_in = v_in
self.v_out = v_out self.v_out = v_out
self.max_power = v_out / float(v_in) self.max_power = v_out / float(v_in)
self.distance_sensor = None
self.auto_mode = False
self.zb = ZeroBorg.ZeroBorg() self.zb = ZeroBorg.ZeroBorg()
self.zb.Init() self.zb.Init()
@ -44,6 +49,19 @@ class GpioZeroborgPlugin(Plugin):
self.zb.ResetEpo() self.zb.ResetEpo()
def get_distance(self):
if not self.distance_sensor:
self.distance_sensor = get_plugin('gpio.sensor.distance')
if not self.distance_sensor:
raise RuntimeError('No gpio.sensor.distance configuration found')
if self.distance_sensor.get_distance() is None:
self.distance_sensor.start()
return self.distance_sensor.get_distance()
def drive(self, direction): def drive(self, direction):
self._can_run = True self._can_run = True
self._direction = direction.lower() self._direction = direction.lower()
@ -54,6 +72,22 @@ class GpioZeroborgPlugin(Plugin):
left = 0.0 left = 0.0
right = 0.0 right = 0.0
if self._direction == Direction.DIR_AUTO.value:
self.auto_mode = True
if self.auto_mode:
distance = None
while distance is None:
distance = self.get_distance()
print(distance)
if distance > 200.0: # distance in mm
self._direction = Direction.DIR_UP.value
else:
self._direction = Direction.DIR_LEFT.value
time.sleep(0.1)
if self._direction == Direction.DIR_UP.value: if self._direction == Direction.DIR_UP.value:
left = 1.0 + self._power_offsets[Direction.DIR_LEFT][Direction.DIR_UP] left = 1.0 + self._power_offsets[Direction.DIR_LEFT][Direction.DIR_UP]
right = 1.0 + self._power_offsets[Direction.DIR_RIGHT][Direction.DIR_UP] right = 1.0 + self._power_offsets[Direction.DIR_RIGHT][Direction.DIR_UP]
@ -65,15 +99,18 @@ class GpioZeroborgPlugin(Plugin):
right = -1.25 - self._power_offsets[Direction.DIR_RIGHT][Direction.DIR_DOWN] right = -1.25 - self._power_offsets[Direction.DIR_RIGHT][Direction.DIR_DOWN]
elif self._direction == Direction.DIR_RIGHT.value: elif self._direction == Direction.DIR_RIGHT.value:
left = -1.25 - self._power_offsets[Direction.DIR_LEFT][Direction.DIR_DOWN] left = -1.25 - self._power_offsets[Direction.DIR_LEFT][Direction.DIR_DOWN]
right = 2 + self._power_offsets[Direction.DIR_RIGHT][Direction.DIR_UP] right = 2.0 + self._power_offsets[Direction.DIR_RIGHT][Direction.DIR_UP]
elif self._direction is not None: elif self._direction is not None:
logging.warning('Invalid direction: {}'.format(direction)) logging.warning('Invalid direction: {}'.format(direction))
self.stop()
self.zb.SetMotor1(left * self.max_power) self.zb.SetMotor1(left * self.max_power)
self.zb.SetMotor2(left * self.max_power) self.zb.SetMotor2(left * self.max_power)
self.zb.SetMotor3(-right * self.max_power) self.zb.SetMotor3(-right * self.max_power)
self.zb.SetMotor4(-right * self.max_power) self.zb.SetMotor4(-right * self.max_power)
self.auto_mode = False
self._drive_thread = threading.Thread(target=_run) self._drive_thread = threading.Thread(target=_run)
self._drive_thread.start() self._drive_thread.start()
@ -88,6 +125,10 @@ class GpioZeroborgPlugin(Plugin):
self.zb.MotorsOff() self.zb.MotorsOff()
self.zb.ResetEpo() self.zb.ResetEpo()
if self.distance_sensor:
self.distance_sensor.stop()
return Response(output={'status':'stopped'}) return Response(output={'status':'stopped'})