feat: added some features

control/dc_motor_controller.py

@@ -0,0 +1,71 @@
+import atexit
+
+try:
+    from RPi import GPIO
+except ImportError:
+    from .mockGpio import GPIO
+
+
+PWM_FREQUENCY = 10_000  # 10 KHz
+MOTORS = []
+
+
+def setup_gpio():
+    GPIO.setmode(GPIO.BCM)
+
+    atexit.register(cleanup_gpio)
+
+
+def cleanup_gpio():
+    print("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:
+            print(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)