diff --git a/config/i2c_servo.yml b/config/i2c_servo.yml deleted file mode 100644 index fab21bad..00000000 --- a/config/i2c_servo.yml +++ /dev/null @@ -1,11 +0,0 @@ -i2c_servo: - enabled: false - path: modules.i2c_servo.I2CServo - config: - test_on_boot: false - servo_count: 16 - dependencies: - unix: - - python3-smbus - python: - - adafruit-circuitpython-servokit \ No newline at end of file diff --git a/config/piservo.yml b/config/piservo.yml deleted file mode 100644 index 2ee4b7b3..00000000 --- a/config/piservo.yml +++ /dev/null @@ -1,12 +0,0 @@ -piservo: - enabled: false - path: modules.actuators.piservo.PiServo - instances: - - name: "ear" - id: 0 - pin: 12 - range: [-40, 40] - start: 0 - dependencies: - python: - - gpiozero diff --git a/config/servos.yml b/config/servos.yml index 25f6cf0f..f2bb9717 100644 --- a/config/servos.yml +++ b/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 \ No newline at end of file + - 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 \ No newline at end of file diff --git a/modules/actuators/servo.py b/modules/actuators/base_servo.py similarity index 61% rename from modules/actuators/servo.py rename to modules/actuators/base_servo.py index 6e056b3a..b9023d5a 100644 --- a/modules/actuators/servo.py +++ b/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::mvabs' to move servo to absolute position - - Argument: percentage (int) - percentage to move servo - - Subscribes to 'servo::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) \ No newline at end of file diff --git a/modules/actuators/gpio_servo.py b/modules/actuators/gpio_servo.py new file mode 100644 index 00000000..790a1831 --- /dev/null +++ b/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() \ No newline at end of file diff --git a/modules/i2c_servo.py b/modules/actuators/i2c_servo.py similarity index 51% rename from modules/i2c_servo.py rename to modules/actuators/i2c_servo.py index ff133ca3..3548c079 100644 --- a/modules/i2c_servo.py +++ b/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 diff --git a/modules/actuators/piservo.py b/modules/actuators/piservo.py deleted file mode 100644 index fce2818e..00000000 --- a/modules/actuators/piservo.py +++ /dev/null @@ -1,46 +0,0 @@ -from gpiozero import AngularServo -from modules.config import Config -from time import sleep -from modules.base_module import BaseModule - -class PiServo(BaseModule): - - def __init__(self, **kwargs): - """ - PiServo class - :param kwargs: pin, range, start - :param pin: GPIO pin number - :param range: [min, max] angle range - :param start: initial angle - - Install: pip install gpiozero - - Subscribes to 'piservo:move' to move servo - - Argument: angle (int) - angle to move servo - - Example: - self.publish('piservo:move', angle=90) - """ - - self.pin = kwargs.get('pin') - self.range = kwargs.get('range') - self.start = kwargs.get('start', 0) - self.servo = None - # print(self.range) - # self.move(0) - # sleep(2) - # self.move(self.range[0]) - # sleep(2) - # self.move(self.range[1]) - # sleep(2) - self.move(self.start) - - def setup_messaging(self): - self.subscribe('piservo:move', self.move) - - def move(self, angle): - if self.servo is None: - self.servo = AngularServo(self.pin, min_angle=self.range[0], max_angle=self.range[1], initial_angle=self.start) - self.servo.angle = angle # Changes the angle (to move the servo) - sleep(1) # @TODO: Remove this sleep - self.servo.detach() # Detaches the servo (to stop jitter) \ No newline at end of file diff --git a/modules/actuators/uart_servo.py b/modules/actuators/uart_servo.py new file mode 100644 index 00000000..8c08f2ec --- /dev/null +++ b/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) \ No newline at end of file