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