Single Servo Module

config/servos.yml

@@ -1,6 +1,6 @@
-servos:
+uart_servos:
   enabled: false
-  path: "modules.actuators.servo.Servo" # Include class name here
+  path: "modules.actuators.uart_servo.UARTServo" # Include class name here
   instances:
     - name: "leg_l_hip"
       id: 0
@@ -44,4 +44,29 @@ servos:
       start: 50
   dependencies:
     python:
-      - pigpio
+      - pigpio
+
+gpio_servos:
+  enabled: false
+  path: "modules.actuators.gpio_servo.GPIOServo" # Include class name here
+  instances:
+    - name: "ear"
+      id: 0
+      pin: 12 
+      range: [-40, 40]
+      start: 0
+  dependencies:
+    python:
+      - gpiozero
+
+i2c_servos:
+  enabled: false
+  path: modules.i2c_servo.I2CServo
+  config:
+    test_on_boot: false
+    servo_count: 16
+  dependencies:
+    unix:
+      - python3-smbus
+    python:
+      - adafruit-circuitpython-servokit

modules/actuators/servo.py → modules/actuators/base_servo.py

@@ -1,18 +1,15 @@
-import pigpio
 import threading
-from modules.config import Config
-from modules.network.arduinoserial import ArduinoSerial
 from time import sleep
 from modules.base_module import BaseModule
 
-class Servo(BaseModule):
+class BaseServo(BaseModule):
 
     def __init__(self, **kwargs):
         """
-        Servo class
-        Uses ArduinoSerial to communicate with Arduino
+        BaseServo class
+        Handles generic functionality for all servo types
 
-        :param kwargs: pin, name, id, range, power, start_pos, buffer, delta, serial, pi
+        :param kwargs: pin, name, id, range, power, start_pos, buffer, delta
         :param pin: GPIO pin number
         :param name: servo name
         :param id: servo id
@@ -21,19 +18,6 @@ def __init__(self, **kwargs):
         :param start_pos: initial angle
         :param buffer: PWM amount to specify as acceleration / deceleration buffer
         :param delta: amount of change in acceleration / deceleration (as a multiple)
-        :param serial: True to use serial connection
-
-        Install: pip install pigpio (optional, untested in this version)
-
-        Subscribes to 'servo:<name>:mvabs' to move servo to absolute position
-        - Argument: percentage (int) - percentage to move servo
-
-        Subscribes to 'servo:<name>:mv' to move servo to relative position
-        - Argument: percentage (int) - percentage to move servo
-
-        Example:
-        self.publish('servo:pan:mvabs', percentage=90)
-        self.publish('servo:pan:mv', percentage=10)
         """
         self.pin = kwargs.get('pin')
         self.identifier = kwargs.get('name')
@@ -46,41 +30,25 @@ def __init__(self, **kwargs):
         self.buffer = kwargs.get('buffer', 0)  # PWM amount to specify as acceleration / deceleration buffer
         self.delta = kwargs.get('delta', 1.5)  # amount of change in acceleration / deceleration (as a multiple)
 
-        # Accepts either serial connection or PI pin
-        self.serial = kwargs.get('serial', True)
-        if self.serial is None:
-            self.pi = kwargs.get('pi', pigpio.pi())
-            self.pi.set_mode(self.pin, pigpio.OUTPUT)
-
         self.move(self.start)
 
     def setup_messaging(self):
         self.subscribe('servo:' + self.identifier + ':mvabs', self.move)
         self.subscribe('servo:' + self.identifier + ':mv', self.move_relative)
 
-    def __del__(self):
-        pass #self.reset()
-
     def move_relative(self, percentage, safe=True):
-        # Only calculate relative position if not using serial
-        # Otherwise it is done by the arduino
-        if not self.serial:
-            new = self.pos + (self.translate(percentage) - self.range[0])
-
-
-            if new > self.range[1] and safe:
-                new = self.range[1]
-            elif new < self.range[0] and safe:
-                new = self.range[0]
-            if self.range[0] <= new <= self.range[1]:
-                this_move = self.calculate_move(self.pos, new)
-                self.execute_move(this_move, True)
-                self.pos = new
-            else:
-                self.publish('log/error', '[Servo] Percentage %d out of range' % percentage)
-                raise ValueError('Percentage %d out of range' % percentage)
+        new = self.pos + (self.translate(percentage) - self.range[0])
+        if new > self.range[1] and safe:
+            new = self.range[1]
+        elif new < self.range[0] and safe:
+            new = self.range[0]
+        if self.range[0] <= new <= self.range[1]:
+            this_move = self.calculate_move(self.pos, new)
+            self.execute_move(this_move, True)
+            self.pos = new
         else:
-            self.execute_move([(percentage, 0)], True)
+            self.publish('log/error', '[Servo] Percentage %d out of range' % percentage)
+            raise ValueError('Percentage %d out of range' % percentage)
 
     def move(self, percentage, safe=True):
         if 0 <= percentage <= 100 or safe:
@@ -106,31 +74,18 @@ def execute_move(self, sequence, is_relative=False):
         print('Moving servo...')
         s = sequence.pop(0)
 
-        # @todo this prevents initial set of position
-        # # ignore request if position matches current position
-        # if s[0] == self.pos:
-        #     return
-
         if self.power:
             self.publish('power:use')
-        if self.serial:
-            # just move the pan servo for now. Remove after debugging
-            # if self.index != 7 and self.index != 6 and self.index != 5 and self.index != 4  and self.index != 3  and self.index != 2:
-                # return
-            type = ArduinoSerial.DEVICE_SERVO
-            if is_relative:
-                type = ArduinoSerial.DEVICE_SERVO_RELATIVE
-            self.publish('serial', type=type, identifier=self.index, message=s[0])
-        else:
-            self.pi.set_servo_pulsewidth(self.pin, s[0])
+        self.perform_move(s[0])
         if len(sequence) > 1:
             timer = threading.Timer(s[1], self.execute_move, [sequence])
             timer.start()
-        else:
-            pass #sleep(s[1])
         if self.power and self.pos == self.start:
             self.publish('power:release')
 
+    def perform_move(self, position):
+        raise NotImplementedError("Subclasses should implement this!")
+
     def calculate_move(self, old, new, time=0.1, translate=False):
         if translate:
             old = self.translate(old)
@@ -151,8 +106,6 @@ def calculate_move(self, old, new, time=0.1, translate=False):
             if current == new:
                 return sequence
 
-            # Accelerate / Decelerate
-            # @todo simplify
             if old < new:
                 if increment < self.buffer and not decelerate:
                     increment = increment * self.delta if increment * self.delta < self.buffer else self.buffer
@@ -178,12 +131,7 @@ def reset(self):
         self.move(self.start)
 
     def translate(self, value):
-        # Figure out how 'wide' each range is
         leftSpan = 100 - 0
         rightSpan = self.range[1] - self.range[0]
-
-        # Convert the left range into a 0-1 range (float)
         valueScaled = float(value) / float(leftSpan)
-
-        # Convert the 0-1 range into a value in the right range.
-        return self.range[0] + (valueScaled * rightSpan)
+        return self.range[0] + (valueScaled * rightSpan)

modules/actuators/gpio_servo.py

@@ -0,0 +1,19 @@
+from gpiozero import AngularServo
+from modules.actuators.base_servo.py import BaseServo
+
+class GPIOServo(BaseServo):
+
+    def __init__(self, **kwargs):
+        """
+        GPIOServo class
+        Uses GPIO to control servos directly from Raspberry Pi
+
+        :param kwargs: pin, name, id, range, power, start_pos, buffer, delta
+        """
+        super().__init__(**kwargs)
+        self.servo = AngularServo(self.pin, min_angle=self.range[0], max_angle=self.range[1], initial_angle=self.start)
+
+    def perform_move(self, position):
+        self.servo.angle = position
+        sleep(1)
+        self.servo.detach()

modules/i2c_servo.py → modules/actuators/i2c_servo.py

@@ -1,21 +1,26 @@
-import os
-from time import sleep
-from modules.base_module import BaseModule
-
 from adafruit_servokit import ServoKit
+from modules.actuators.base_servo.py import BaseServo
+
+class I2CServo(BaseServo):
 
-# @todo: Merge with main servo module
-class I2CServo(BaseModule):
     def __init__(self, **kwargs):
-        # Scan with `sudo i2cdetect -y 1`
+        """
+        I2CServo class
+        Uses I2C to control servos via PCA9685 driver board
+
+        :param kwargs: pin, name, id, range, power, start_pos, buffer, delta, servo_count, test_on_boot
+        """
+        super().__init__(**kwargs)
         self.servos = ServoKit(channels=16)
-        self.count = kwargs.get('servo_count')
-        # https://learn.adafruit.com/adafruit-16-channel-servo-driver-with-raspberry-pi/using-the-adafruit-library
+        self.count = kwargs.get('servo_count', 16)  # @todo: change to reference a single servo. Research how to achieve this with ServoKit
         for i in range(self.count):
             self.moveServo(i, 90)
         if kwargs.get('test_on_boot'):
             self.test()
-
+
+    def perform_move(self, position):
+        self.servos.servo[self.index].angle = position
+
     def moveServo(self, index, angle):
         self.servos.servo[index].angle = angle
 

modules/actuators/uart_servo.py

@@ -0,0 +1,25 @@
+import pigpio
+from modules.network.arduinoserial import ArduinoSerial
+from modules.actuators.base_servo.py import BaseServo
+
+class UARTServo(BaseServo):
+
+    def __init__(self, **kwargs):
+        """
+        UARTServo class
+        Uses ArduinoSerial to communicate with Arduino
+
+        :param kwargs: pin, name, id, range, power, start_pos, buffer, delta, serial, pi
+        """
+        super().__init__(**kwargs)
+        self.serial = kwargs.get('serial', True)
+        if self.serial is None:
+            self.pi = kwargs.get('pi', pigpio.pi())
+            self.pi.set_mode(self.pin, pigpio.OUTPUT)
+
+    def perform_move(self, position):
+        if self.serial:
+            type = ArduinoSerial.DEVICE_SERVO
+            self.publish('serial', type=type, identifier=self.index, message=position)
+        else:
+            self.pi.set_servo_pulsewidth(self.pin, position)