Friday, February 17, 2017

Python Servo Module/Library for Raspberry Pi

Python Servo Library

We will now create a servo wrapper class which will add some convenience methods and attributes using software PWM. It will be designed so that it can be imported into other code or used by itself to calibrate or test a servo.

From here we will start putting together a Python Robot class similar to what we did for AVA in C++.

This library is based on the code we used to calibrate the sensor and still has some jitter. In due course we will look at improving the code using hardware DMA (perhaps using the pigpio library this time).

The code is well commented, so with no further ado, here it is.

# - Wrapper Servo Class for Raspberry Pi
# 15 February 2017 - 1.0 Original Issue
# 18 February 2017 - 1.1 Modified with @property
# Reefwing Software

import RPi.GPIO as GPIO
from time import sleep

# Private Attributes
__CALIBRATE      = "1"
__SCAN           = "3"
__QUIT           = "q"

class Servo:
    # Servo class wrapper using RPi.GPIO PWM

    # Servo private class attribute - to count servo instances
    __number = 0

    def __init__(self, pin, min_dc=2, max_dc=10, freq=50):
        # Create a new servo instance with default pulse width limits if not provided

        # Configure the Pi to use pin names (i.e. BCM) and allocate I/O
        GPIO.setup(pin, GPIO.OUT)

        # Create PWM channel on the servo pin with a frequency of freq - default 50Hz
        self.PWM = GPIO.PWM(pin, freq)

        # Increment Servo instances
        type(self).__number += 1     
        # Instance attributes = pin
        self.min_duty_cycle = min_dc
        self.max_duty_cycle = max_dc
        self.duty_cycle = self.get_centre(min_dc, max_dc)
        self.angle = 0

    def __str__(self):
        # Return string representation of servo
        return "Servo: pin - {0}, MIN_DC - {1}, MAX_DC - {2}, DC - {3}".format(, self.min_duty_cycle, self.max_duty_cycle, self.duty_cycle)

    def start(self):
        # Start PWM

    def stop(self):
        # Stop PWM

    def centre(self):
        # Move servo to the centre position
        centre = self.get_centre(self.min_duty_cycle, self.max_duty_cycle)
        self.duty_cycle = centre

    def min_dc(self):
        # Move servo to minimum duty cycle position
        self.duty_cycle = self.min_duty_cycle

    def max_dc(self):
        # Move servo to maximum duty cycle position
        self.duty_cycle = self.max_duty_cycle

    def scan(self, min_dc=None, max_dc=None):
        # Scans from min_dc to max_dc - defaults to max and min duty cycle
        min_dc = (min_dc or self.min_duty_cycle)
        max_dc = (max_dc or self.max_duty_cycle)
        centre = self.get_centre(min_dc, max_dc)
        self.duty_cycle = min_dc
        self.duty_cycle = centre
        self.duty_cycle = max_dc
        self.duty_cycle = centre

    def cal_duty_cycle(self, dc):
        # Set duty cycle for servo - not clamped

    def cleanup(self):
        # Stop PWM channel for servo and centre

    def duty_cycle(self):
        return self.__duty_cycle

    def duty_cycle(self, dc):
        # Set duty cycle for servo - clamped to max and min duty cycle
        dc = self.clamp(dc, self.min_duty_cycle, self.max_duty_cycle)
        self.__duty_cycle = dc

    def get_centre(min_dc, max_dc):
         return min_dc + (max_dc - min_dc)/2

    def clamp(dc, min_dc, max_dc):
        return max(min(dc, max_dc), min_dc)
    def count(cls):
        # Returns the number of Servo instances
        return cls.__number

def main():
        servo = Servo(6)    # create a new servo to be controlled from GPIO pin 5
        servo.start()       # start PWM for servo

        print("Servo instances: ", Servo.count())

        while True:
            test = input("\nSelect Action - (1) Calibrate, (2) Set max/min duty cycle, or (3) Scan: ")

            if test == __CALIBRATE:
                while True:
                    response = input("Enter Duty Cycle (q = quit): ")
                    if response == __QUIT:
            elif test == __SET_DUTY_CYCLE:
                min_dc = float(input("Enter minimum duty cycle: "))
                max_dc = float(input("Enter maximum duty cycle: "))
                servo.min_duty_cycle = min_dc
                servo.max_duty_cycle = max_dc
                while True:
                    # Scan Servo from min duty cycle to max duty cycle
                    print("\nScanning (CTRL c to exit)...")
    except KeyboardInterrupt:
        print("\n-- CTRL-C: Terminating program --")
        print("Cleaning up PWM and GPIO...")

if __name__ == "__main__":
    # execute only if run as a script

No comments:

Post a Comment