An exploration of PID control external to that provided by the AX-12 Dynamixel servo.

externalPIDControl.py

#!/usr/bin/env python

import os
import dynamixel
import time
import sys
import subprocess
import optparse
import yaml

"""
This script externalizes the Proportional-Integral-Derivative (PID) control
loop of the servo. This is for educational purposes mainly.
"""

VERBOSITY = 1

def main(settings):

    ##########################################################################
    # Servo Network Setup
    portName = settings['port']
    baudRate = settings['baudRate']
    
    # Establish a serial connection to the dynamixel network.
    # This usually requires a USB2Dynamixel
    serial = dynamixel.SerialStream(port=portName,
                                    baudrate=baudRate,
                                    timeout=1)
    net = dynamixel.DynamixelNetwork(serial)
    
    ##########################################################################
    # Single Servo Setup
    servoID = 12
    net.scan(servoID, servoID)
    servo = net.get_dynamixels()[0]
    servo.ccw_compliance_slope = servo.cw_compliance_slope = 8
    servo.punch = 8
    servo.moving_speed = 512
    servo.synchronized = True
    servo.torque_enable = True
    servo.torque_limit = 400
    
    ##########################################################################
    # Initial Position
    servo.goal_position = 1023
    net.synchronize()
    time.sleep(1)
    
    ##########################################################################
    # Control Loop Setup
    targetPosition = 300
    # We ignore errors within +/- this range.
    margin = 4
    # Max and min values of possible positions (inherent to the servo)
    maxP = 1023
    minP = 0
    
    ##############################
    # Proportional (P) Paramaters
    
    # The torque will be increased by error * propGainConstant
    propGainConstant = 5
    
    ##############################
    # Integral (I) Paramaters
    window = []
    windowWidth = 40
    integralContribution = 0
    integralGain = .5

    ##############################
    # Derivative (D) Paramaters
    tPrev = None
    ePrev = None
    derivativeGain = .1
    derivativeContribution = 0
    
    # Go as fast as possible!
    while True:

      # Find the time we are starting to take the error measurment
      tNow = time.time()
      # If this is the first measurment tDelta will be zero
      if not tPrev:
        tPrev = tNow
      tDelta = tNow - tPrev
      # Update for next iteration
      tPrev = tNow
      
      # If we've outgrown our window, remove the oldest value
      windowSize = len(window)
      if windowSize > windowWidth:
        window.pop(0)
      
      # Get the current position
      curP = servo.current_position
      
      # Our base error in this sample
      error = targetPosition - curP
      absError = abs(error)
      if not ePrev:
        ePrev = absError

      # Proportional torque
      propTorque = error * propGainConstant
      # Limit to allowed servo range
      propTorque = min( (abs(propTorque), 1023) )
      
      # Exponential proportional torque
      # propTorque = min( (abs(error * (error / 2)), 500))

      # Add that to our sliding window for use in integral calculation
      window.append((absError, tDelta))
      
      # The sum of errors * their time delta in our window
      integralTotal = sum([e*td for e, td in window])
      # Here's where the integral tuning parameter comes in.
      integralContribution = integralTotal * integralGain
      
      # Derivative contribution
      if tDelta:
        eDelta = absError - ePrev
        # Update for next iteration
        ePrev = absError
        errorSlope = eDelta / tDelta
        derivativeContribution = derivativeGain * errorSlope

      # Sum the contributions
      finalTorque = propTorque + integralContribution + derivativeContribution
      # Convert to a whole number
      finalTorque = int(round(finalTorque))
      
      # Determind which way we need to move
      if curP > targetPosition + margin:
        servo.goal_position = minP
        servo.torque_limit = finalTorque
      elif curP < targetPosition - margin:
        servo.goal_position = maxP
        servo.torque_limit = finalTorque
      else:
        servo.torque_limit = 0
      net.synchronize()
      if VERBOSITY >= 1:
        print '=' * 60
        print "INTERNAL"
        print "Servo Goal Position:", servo.goal_position
        print "Servo Torque Limit:", servo.torque_limit
        print "EXTERNAL"
        print "Target Positon:", targetPosition
        print "Curent Position:", curP
        print "Error:", error
        print "Proportional Torque:", propTorque
        print "Integral Contribution:", integralContribution
        try:
          print "Error Slope:", errorSlope
          print "Derivative Contribution:", derivativeContribution
        except UnboundLocalError:
          pass
        print "Final Torque:", finalTorque
        print "Servo Load:", servo.current_load
        
def validateInput(userInput, rangeMin, rangeMax):
    '''
    Returns valid user input or None
    '''
    try:
        inTest = int(userInput)
        if inTest < rangeMin or inTest > rangeMax:
            print "ERROR: Value out of range [" + str(rangeMin) + '-' + \
                                               str(rangeMax) + "]"
            return None
    except ValueError:
        print("ERROR: Please enter an integer")
        return None
    
    return inTest

if __name__ == '__main__':
    
    parser = optparse.OptionParser()
    parser.add_option("-c", "--clean",
                      action="store_true", dest="clean", default=False,
                      help="Ignore the settings.yaml file if it exists and \
                      prompt for new settings.")
    
    (options, args) = parser.parse_args()
    
    # Look for a settings.yaml file
    settingsFile = 'settings.yaml'
    if not options.clean and os.path.exists(settingsFile):
        with open(settingsFile, 'r') as fh:
            settings = yaml.load(fh)
    # If we were asked to bypass, or don't have settings
    else:
        settings = {}
        if os.name == "posix":
            portPrompt = "Which port corresponds to your USB2Dynamixel? \n"
            # Get a list of ports that mention USB
            try:
                possiblePorts = subprocess.check_output('ls /dev/ | grep -i usb',
                                                        shell=True).split()
                possiblePorts = ['/dev/' + port for port in possiblePorts]
            except subprocess.CalledProcessError:
                sys.exit("USB2Dynamixel not found. Please connect one.")
                
            counter = 1
            portCount = len(possiblePorts)
            for port in possiblePorts:
                portPrompt += "\t" + str(counter) + " - " + port + "\n"
                counter += 1
            portPrompt += "Enter Choice: "
            portChoice = None
            while not portChoice:
                portTest = raw_input(portPrompt)
                portTest = validateInput(portTest, 1, portCount)
                if portTest:
                    portChoice = possiblePorts[portTest - 1]
        else:
            portPrompt = "Please enter the USB2Dynamixel port name: "
            portChoice = raw_input(portPrompt)
            
        settings['port'] = portChoice
        
        # Baud rate
        baudRate = None
        while not baudRate:
            brTest = raw_input("Enter baud rate [Default: 1000000 bps]:")
            if not brTest:
                baudRate = 1000000
            else:
                baudRate = validateInput(brTest, 9600, 1000000)
                    
        settings['baudRate'] = baudRate
        
        # Save the output settings to a yaml file
        with open(settingsFile, 'w') as fh:
            yaml.dump(settings, fh)
            print("Your settings have been saved to 'settings.yaml'. \nTo " +
                   "change them in the future either edit that file or run " +
                   "this example with -c.")
    
    main(settings)