2018-02-25 23:45:31 +01:00
|
|
|
import enum
|
|
|
|
import threading
|
|
|
|
import time
|
|
|
|
|
2019-12-23 21:40:30 +01:00
|
|
|
from typing import Dict, List
|
2019-12-22 23:54:45 +01:00
|
|
|
|
|
|
|
from platypush.context import get_bus
|
|
|
|
from platypush.message.event.zeroborg import ZeroborgDriveEvent, ZeroborgStopEvent
|
2018-07-06 02:08:38 +02:00
|
|
|
from platypush.plugins import Plugin, action
|
2018-02-25 23:45:31 +01:00
|
|
|
|
|
|
|
|
|
|
|
class Direction(enum.Enum):
|
|
|
|
DIR_UP = 'up'
|
|
|
|
DIR_DOWN = 'down'
|
|
|
|
DIR_LEFT = 'left'
|
|
|
|
DIR_RIGHT = 'right'
|
|
|
|
|
|
|
|
|
2019-07-02 14:04:25 +02:00
|
|
|
# noinspection PyPep8Naming
|
2018-02-25 23:45:31 +01:00
|
|
|
class GpioZeroborgPlugin(Plugin):
|
2018-06-25 00:49:45 +02:00
|
|
|
"""
|
|
|
|
ZeroBorg plugin. It allows you to control a ZeroBorg
|
|
|
|
(https://www.piborg.org/motor-control-1135/zeroborg) motor controller and
|
|
|
|
infrared sensor circuitry for Raspberry Pi
|
|
|
|
|
2019-12-22 23:54:45 +01:00
|
|
|
Triggers:
|
|
|
|
|
|
|
|
* :class:`platypush.message.event.zeroborg.ZeroborgDriveEvent` when motors direction changes
|
|
|
|
* :class:`platypush.message.event.zeroborg.ZeroborgStopEvent` upon motors stop
|
|
|
|
|
|
|
|
"""
|
2018-02-25 23:45:31 +01:00
|
|
|
|
2019-12-23 17:57:35 +01:00
|
|
|
def __init__(self, directions: Dict[str, List[float]] = None, **kwargs):
|
2018-06-25 00:49:45 +02:00
|
|
|
"""
|
2019-07-02 14:04:25 +02:00
|
|
|
:param directions: Configuration for the motor directions. A direction is basically a configuration of the
|
|
|
|
power delivered to each motor to allow whichever object you're controlling (wheels, robotic arms etc.) to
|
|
|
|
move in a certain direction. In my experience the ZeroBorg always needs a bit of calibration, depending on
|
|
|
|
factory defaults and the mechanical properties of the load it controls.
|
2018-07-16 22:58:56 +02:00
|
|
|
|
2018-06-25 00:49:45 +02:00
|
|
|
Example configuration that I use to control a simple 4WD robot::
|
|
|
|
|
|
|
|
directions = {
|
2019-12-23 17:57:35 +01:00
|
|
|
"up": [
|
|
|
|
0.4821428571428572, # Motor 1 power
|
|
|
|
0.4821428571428572, # Motor 2 power
|
|
|
|
-0.6707142857142858, # Motor 3 power
|
|
|
|
-0.6707142857142858 # Motor 4 power
|
|
|
|
],
|
|
|
|
"down": [
|
|
|
|
-0.4821428571428572,
|
|
|
|
-0.4821428571428572,
|
|
|
|
0.825,
|
|
|
|
0.825
|
|
|
|
],
|
|
|
|
"left": [
|
|
|
|
-0.1392857142857143,
|
|
|
|
-0.1392857142857143,
|
|
|
|
-1.0553571428571429,
|
|
|
|
-1.0553571428571429
|
|
|
|
],
|
|
|
|
"right": [
|
|
|
|
1.0017857142857143,
|
|
|
|
1.0017857142857143,
|
|
|
|
0.6214285714285713,
|
|
|
|
0.6214285714285713
|
|
|
|
]
|
2018-06-25 00:49:45 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
"""
|
|
|
|
|
2019-07-02 14:04:25 +02:00
|
|
|
if directions is None:
|
|
|
|
directions = {}
|
|
|
|
|
2018-06-08 17:10:44 +02:00
|
|
|
import platypush.plugins.gpio.zeroborg.lib as ZeroBorg
|
2019-07-02 14:04:25 +02:00
|
|
|
super().__init__(**kwargs)
|
2018-02-25 23:45:31 +01:00
|
|
|
|
2018-04-13 15:12:31 +02:00
|
|
|
self.directions = directions
|
2018-03-03 03:24:08 +01:00
|
|
|
self._direction = None
|
2019-12-23 21:40:30 +01:00
|
|
|
self._drive_thread = None
|
2019-12-23 00:36:53 +01:00
|
|
|
self._motors = [0, 0, 0, 0]
|
2018-02-25 23:45:31 +01:00
|
|
|
|
2018-08-22 02:04:16 +02:00
|
|
|
self.zb = ZeroBorg.ZeroBorg()
|
|
|
|
self.zb.Init()
|
|
|
|
self.zb.SetCommsFailsafe(True)
|
|
|
|
self.zb.ResetEpo()
|
2018-02-25 23:45:31 +01:00
|
|
|
|
2019-07-02 14:04:25 +02:00
|
|
|
@staticmethod
|
|
|
|
def _get_measurement(plugin, timeout):
|
2018-04-13 15:49:10 +02:00
|
|
|
measure_start_time = time.time()
|
|
|
|
value = None
|
2018-02-26 11:31:06 +01:00
|
|
|
|
2018-04-13 15:49:10 +02:00
|
|
|
while value is None:
|
2018-07-06 02:08:38 +02:00
|
|
|
value = plugin.get_measurement().output
|
2018-04-13 15:49:10 +02:00
|
|
|
if time.time() - measure_start_time > timeout:
|
|
|
|
return None
|
|
|
|
|
|
|
|
return value
|
|
|
|
|
2018-07-06 02:08:38 +02:00
|
|
|
@action
|
2018-02-25 23:45:31 +01:00
|
|
|
def drive(self, direction):
|
2018-06-25 00:49:45 +02:00
|
|
|
"""
|
|
|
|
Drive the motors in a certain direction.
|
|
|
|
"""
|
|
|
|
|
2018-02-25 23:45:31 +01:00
|
|
|
def _run():
|
2019-12-21 13:09:44 +01:00
|
|
|
try:
|
2019-12-23 00:36:53 +01:00
|
|
|
while self._direction:
|
2019-12-21 13:09:44 +01:00
|
|
|
try:
|
2019-12-23 17:57:35 +01:00
|
|
|
if self._direction in self.directions:
|
2019-12-23 00:36:53 +01:00
|
|
|
self._motors = self.directions[self._direction]
|
2019-12-21 13:09:44 +01:00
|
|
|
else:
|
|
|
|
self.logger.warning('Invalid direction {}: stopping motors'.format(self._direction))
|
|
|
|
except Exception as e:
|
|
|
|
self.logger.error('Error on _get_direction_from_sensors: {}'.format(str(e)))
|
|
|
|
break
|
|
|
|
|
2019-12-23 00:36:53 +01:00
|
|
|
for i, power in enumerate(self._motors):
|
2019-12-22 23:54:45 +01:00
|
|
|
method = getattr(self.zb, 'SetMotor{}'.format(i+1))
|
|
|
|
method(power)
|
2019-12-21 13:09:44 +01:00
|
|
|
finally:
|
2019-12-23 00:36:53 +01:00
|
|
|
self.zb.MotorsOff()
|
|
|
|
self.zb.ResetEpo()
|
2019-12-22 23:54:45 +01:00
|
|
|
self._drive_thread = None
|
|
|
|
|
|
|
|
self._direction = direction.lower()
|
2018-02-26 11:31:06 +01:00
|
|
|
|
2019-12-22 23:54:45 +01:00
|
|
|
if not self._drive_thread:
|
2019-12-23 18:48:01 +01:00
|
|
|
drive_thread = threading.Thread(target=_run)
|
|
|
|
drive_thread.start()
|
|
|
|
self._drive_thread = drive_thread
|
2018-02-25 23:45:31 +01:00
|
|
|
|
2019-12-23 00:36:53 +01:00
|
|
|
get_bus().post(ZeroborgDriveEvent(direction=self._direction, motors=self.directions[self._direction]))
|
2018-07-06 02:08:38 +02:00
|
|
|
return {'status': 'running', 'direction': direction}
|
2018-02-25 23:45:31 +01:00
|
|
|
|
2018-07-06 02:08:38 +02:00
|
|
|
@action
|
2018-02-25 23:45:31 +01:00
|
|
|
def stop(self):
|
2018-06-25 00:49:45 +02:00
|
|
|
"""
|
|
|
|
Turns off the motors
|
|
|
|
"""
|
|
|
|
|
2019-12-23 00:36:53 +01:00
|
|
|
self._direction = None
|
|
|
|
if self._drive_thread:
|
2018-02-25 23:45:31 +01:00
|
|
|
self._drive_thread.join()
|
|
|
|
|
2019-12-22 23:54:45 +01:00
|
|
|
get_bus().post(ZeroborgStopEvent())
|
2019-07-02 14:04:25 +02:00
|
|
|
return {'status': 'stopped'}
|
2018-02-25 23:45:31 +01:00
|
|
|
|
2019-12-23 00:36:53 +01:00
|
|
|
@action
|
|
|
|
def status(self) -> dict:
|
|
|
|
"""
|
|
|
|
Get the current direction and motors power. Example response::
|
|
|
|
|
2020-01-05 13:11:44 +01:00
|
|
|
.. code-block:: json
|
2019-12-23 00:36:53 +01:00
|
|
|
|
|
|
|
{
|
|
|
|
"status": "running",
|
|
|
|
"direction": "up",
|
|
|
|
"motors": [1.0, 1.0, -1.0, -1.0]
|
|
|
|
}
|
|
|
|
|
|
|
|
"""
|
|
|
|
|
|
|
|
return {
|
|
|
|
'status': 'running' if self._direction else 'stopped',
|
|
|
|
'direction': self._direction,
|
|
|
|
'motors': [getattr(self.zb, 'GetMotor{}'.format(i+1))() for i in range(4)],
|
|
|
|
}
|
|
|
|
|
2018-02-25 23:45:31 +01:00
|
|
|
|
|
|
|
# vim:sw=4:ts=4:et:
|