forked from platypush/platypush
135 lines
4.4 KiB
Python
135 lines
4.4 KiB
Python
import enum
|
|
import logging
|
|
import threading
|
|
import time
|
|
|
|
from platypush.message.response import Response
|
|
from platypush.plugins import Plugin
|
|
from platypush.context import get_plugin
|
|
from platypush.config import Config
|
|
|
|
|
|
class Direction(enum.Enum):
|
|
DIR_UP = 'up'
|
|
DIR_DOWN = 'down'
|
|
DIR_LEFT = 'left'
|
|
DIR_RIGHT = 'right'
|
|
DIR_AUTO = 'auto'
|
|
|
|
|
|
class GpioZeroborgPlugin(Plugin):
|
|
_drive_thread = None
|
|
_can_run = False
|
|
_direction = None
|
|
_power_offsets = {
|
|
Direction.DIR_LEFT: {
|
|
Direction.DIR_UP: 0.315,
|
|
Direction.DIR_DOWN: 0.285,
|
|
},
|
|
|
|
Direction.DIR_RIGHT: {
|
|
Direction.DIR_UP: 0,
|
|
Direction.DIR_DOWN: 0.242,
|
|
},
|
|
}
|
|
|
|
|
|
def __init__(self, v_in=8.4, v_out=6.0):
|
|
import platypush.plugins.gpio.zeroborg.lib as ZeroBorg
|
|
|
|
self.v_in = v_in
|
|
self.v_out = v_out
|
|
self.max_power = v_out / float(v_in)
|
|
self.auto_mode = False
|
|
self._direction = None
|
|
|
|
self.zb = ZeroBorg.ZeroBorg()
|
|
self.zb.Init()
|
|
self.zb.SetCommsFailsafe(True)
|
|
self.zb.ResetEpo()
|
|
|
|
|
|
def get_distance(self):
|
|
distance_sensor = get_plugin('gpio.sensor.distance')
|
|
if not distance_sensor:
|
|
raise RuntimeError('No gpio.sensor.distance configuration found')
|
|
|
|
return distance_sensor.get_distance()
|
|
|
|
|
|
def drive(self, direction):
|
|
prev_direction = self._direction
|
|
|
|
self._can_run = True
|
|
self._direction = direction.lower()
|
|
logging.info('Received ZeroBorg drive command: {}'.format(direction))
|
|
|
|
def _run():
|
|
while self._can_run and self._direction:
|
|
left = 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 > 300.0: # distance in mm
|
|
self._direction = Direction.DIR_UP.value
|
|
else:
|
|
logging.info('Physical obstacle detected at {} mm'.
|
|
format(distance))
|
|
self._direction = Direction.DIR_LEFT.value
|
|
|
|
time.sleep(0.1)
|
|
|
|
if self._direction == Direction.DIR_UP.value:
|
|
left = 1.0 + self._power_offsets[Direction.DIR_LEFT][Direction.DIR_UP]
|
|
right = 1.0 + self._power_offsets[Direction.DIR_RIGHT][Direction.DIR_UP]
|
|
elif self._direction == Direction.DIR_DOWN.value:
|
|
left = -1.0 - self._power_offsets[Direction.DIR_LEFT][Direction.DIR_DOWN]
|
|
right = -1.0 - self._power_offsets[Direction.DIR_RIGHT][Direction.DIR_DOWN]
|
|
elif self._direction == Direction.DIR_LEFT.value:
|
|
left = -1.25 - self._power_offsets[Direction.DIR_LEFT][Direction.DIR_DOWN]
|
|
right = 2.0 + self._power_offsets[Direction.DIR_RIGHT][Direction.DIR_UP]
|
|
elif self._direction == Direction.DIR_RIGHT.value:
|
|
left = 2.0 + self._power_offsets[Direction.DIR_LEFT][Direction.DIR_UP]
|
|
right = -1.25 - self._power_offsets[Direction.DIR_RIGHT][Direction.DIR_DOWN]
|
|
elif self._direction is not None:
|
|
logging.warning('Invalid direction: {}'.format(direction))
|
|
self.stop()
|
|
|
|
power = 0.75 # Still debugging the right power range
|
|
|
|
self.zb.SetMotor1(power * left * self.max_power)
|
|
self.zb.SetMotor2(power * left * self.max_power)
|
|
self.zb.SetMotor3(power * -right * self.max_power)
|
|
self.zb.SetMotor4(power * -right * self.max_power)
|
|
|
|
self.auto_mode = False
|
|
|
|
|
|
self._drive_thread = threading.Thread(target=_run)
|
|
self._drive_thread.start()
|
|
|
|
return Response(output={'status': 'running', 'direction': direction})
|
|
|
|
|
|
def stop(self):
|
|
self._can_run = False
|
|
if self._drive_thread and threading.get_ident() != self._drive_thread.ident:
|
|
self._drive_thread.join()
|
|
|
|
self.zb.MotorsOff()
|
|
self.zb.ResetEpo()
|
|
|
|
return Response(output={'status':'stopped'})
|
|
|
|
|
|
# vim:sw=4:ts=4:et:
|
|
|