74 lines
1.6 KiB
Python
74 lines
1.6 KiB
Python
import atexit
|
|
import logging
|
|
|
|
try:
|
|
from RPi import GPIO
|
|
except ImportError:
|
|
from .mockGpio import GPIO
|
|
|
|
LOG = logging.getLogger(__name__)
|
|
|
|
PWM_FREQUENCY = 10_000 # 10 KHz
|
|
MOTORS = []
|
|
|
|
|
|
def setup_gpio():
|
|
GPIO.setmode(GPIO.BCM)
|
|
|
|
atexit.register(cleanup_gpio)
|
|
|
|
|
|
def cleanup_gpio():
|
|
LOG.info("cleaning up")
|
|
for motor in MOTORS:
|
|
# noinspection PyProtectedMember
|
|
motor._stop()
|
|
GPIO.cleanup()
|
|
|
|
|
|
class DcMotor:
|
|
def __init__(self, pin_enable, pin_reverse):
|
|
self.pin_enable = pin_enable
|
|
self.pin_reverse = pin_reverse
|
|
|
|
GPIO.setup(pin_enable, GPIO.OUT)
|
|
GPIO.setup(pin_reverse, GPIO.OUT)
|
|
GPIO.output(pin_reverse, GPIO.LOW)
|
|
|
|
self.pwm_enable = GPIO.PWM(pin_enable, PWM_FREQUENCY)
|
|
self.pwm_enable.start(0.0)
|
|
self.is_reverse = False
|
|
|
|
MOTORS.append(self)
|
|
|
|
def set(self, val):
|
|
"""
|
|
Set the speed of the motor
|
|
|
|
Args:
|
|
val: Value between 1.0 and -1.0
|
|
"""
|
|
self._set_reverse(val)
|
|
absval = abs(val)
|
|
if absval > 1:
|
|
logging.warning(f"clipping {val} to 1")
|
|
absval = 1
|
|
|
|
self.pwm_enable.ChangeDutyCycle(absval * 100)
|
|
|
|
def _set_reverse(self, val):
|
|
if val >= 0 and self.is_reverse:
|
|
GPIO.output(self.pin_reverse, GPIO.LOW)
|
|
self.is_reverse = False
|
|
elif val < 0 and not self.is_reverse:
|
|
GPIO.output(self.pin_reverse, GPIO.HIGH)
|
|
self.is_reverse = True
|
|
|
|
def _stop(self):
|
|
self.pwm_enable.stop()
|
|
GPIO.output(self.pin_reverse, GPIO.LOW)
|
|
|
|
def stop(self):
|
|
self._stop()
|
|
MOTORS.remove(self)
|