Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
An exploration of PID control external to that provided by the AX-12 Dynamixel servo.
#!/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)
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.