Need help with PID algorithm for closed loop servo position control -- larger servos
laser-vector
Posts: 118
Hi,
It's been a good while since i have worked with the Propeller chip (and must say I'm glad to be back)!
I am using the object "Quadrature Encoder and PID Controller Driver" by Kwabena W. Agyeman to try to control some larger servos (eg&g torque systems 2520).
My issue is that the object was originally developed to control a servo at a constant velocity but i need control of its position.
i tried modifying the object last night to add some simple Spin code that attempts to control the motor's position but
its a bit buggy in some areas and works ok in others..
also i have never written a PID algorithm before (though I have used them many times).
as a note: I'm attempting to get precise positioning of my servos at lower speeds with minimal load.
So This is a modified version of This object.
And the modifications i made where i attempt to control the servo's position reside primarily within the "main()" method.
and this is just the isolated algorithm (same as the one in main() from above without the comments)
I hope that the way i wrote this algorithm makes sense for someone to understand it, i have never written a PID function and only vaguely understand their inner workings (plus time away from the prop chip has made this a bit of a learning experience), so please try not to laugh!!
for fun i included a pic of the system im building:
And thanks for anyone's help!!
[edit]
I would also like to list some of the non-desirable behavior i noticed in it.
*the acceleration and deceleration of my PID loop seem to be dependent on the value of maxspeed, where larger values = longer accel and decel times.
*when the motor is at rest (holding at position), if the motor's shaft is forcefully rotated and held at some error value and then slowly rotated back to where error=0, the motor will continue to apply force until some "value" is reached in the opposite direction where the loop finally seems to kick in correctly. i think this is considered overshoot? however while debugging i can clearly see the value of both PID and speedTarget in my loop approaching zero and flipping signs properly. so i think this has more to do with the way im updating the parameters in the original Asm based PID loop that controls the velocity of the motor.
It's been a good while since i have worked with the Propeller chip (and must say I'm glad to be back)!
I am using the object "Quadrature Encoder and PID Controller Driver" by Kwabena W. Agyeman to try to control some larger servos (eg&g torque systems 2520).
My issue is that the object was originally developed to control a servo at a constant velocity but i need control of its position.
i tried modifying the object last night to add some simple Spin code that attempts to control the motor's position but
its a bit buggy in some areas and works ok in others..
also i have never written a PID algorithm before (though I have used them many times).
as a note: I'm attempting to get precise positioning of my servos at lower speeds with minimal load.
So This is a modified version of This object.
And the modifications i made where i attempt to control the servo's position reside primarily within the "main()" method.
{{
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// PWM1C Proportional Integral Derivative Engine
//
// Author: Kwabena W. Agyeman
// Modified by: Grant Olsen
// Updated: 4/15/2014
// Designed For: P8X32A
// Version: 1.0-G_Mod
//
// Copyright (c) 2010 Kwabena W. Agyeman
// See end of file for terms of use.
//
// Update History:
//
//
// v1.0 - Original release - 10/15/2010.
//
// For each included copy of this object only one spin interpreter should acess it at a time.
//
// Nyamekye,
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Feedback Circuit:
//
// Motor Left - To H-Bridge, driven by the propeller chip.
//
// Motor Right - To H-Bridge, driven by the propeller chip.
//
// Encoder Left - From the quadrature encoder, driven by the encoder.
//
// Encoder Right - From the quadrature encoder, driven by the encoder.
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
}}
CON
'THIS SECTION HAS BEEN ADDED TO ORIGINAL OBJECT!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
_clkmode = xtal1 + pll16x
_xinfreq = 5_000_000
LED_PIN = 24
_PGain = 27 'connected to velocity control PID (AKA original object's PID loop)
_IGain = 16 'connected to velocity control PID
_Dgain = 200'connected to velocity control PID
P_Gain = 14 'connected to position control PID (AKA laser-vector's sort of PID loop thing)
I_Gain = 8 'connected to position control PID
D_Gain = 150 'connected to position control PID
_encoderRightInput = 22
_encoderLeftInput = 23
_motorLeftOutput = 28
_motorRightOutput = 27
VAR
long cogNumber, positionMeasurement, deltaPositionMeasurement, speedTarget, currentError
long posTarget 'VARIABLE HAS BEEN ADDED TO ORIGINAL OBJECT!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
OBJ
'THIS SECTION HAS BEEN ADDED TO ORIGINAL OBJECT!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
ser : "FullDuplexSerial"
PUB main | sign, PID, P, I, error, startPosition, resolution, lastTarget, maxSpeed
'THIS METHOD HAS BEEN ADDED TO ORIGINAL OBJECT!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
'----initialize vars----
dira[LED_PIN] := True
lastTarget := 0 'used in calculating relative positions
maxSpeed := 1000 'ticks per second limit
speedTarget := 0 'updated by the PID loop to tell the velocity PID what speed the motor should be rotating
posTarget := -3000 'set the target position,
'----Start processes----
'Start the velocity control asm PID engine
PIDEngineStart(_motorLeftOutput, _motorRightOutput, _encoderLeftInput, _encoderRightInput, _PGain, _IGain, _DGain)
'Start serial for debugging purposes
ser.Start(31,30,0,9600)
'----Main Loop----
waitcnt(clkfreq*1 +cnt) 'Motor settling time
repeat
ifnot posTarget == lastTarget 'fires when the target position is changed
lastTarget := posTarget
startPosition := positionMeasurement
error := ( posTarget - positionMeasurement ) 'calculate the error (can be +/- depending on direction)
P := P_Gain * error 'High error gain
I := ( ( || (posTarget - startPosition) - || error ) + D_Gain) * I_Gain #> 1
PID := P / ( ( ( maxSpeed * 100 ) / I ) #> 1 )
PID := PID <# maxSpeed #> maxSpeed*-1
speedTarget := PID 'write value into speedTarget for velocity control asm PID loop
'----Debuging Spam----
waitcnt(clkfreq/100 +cnt)
ser.str(string("P:"))
ser.Dec( speedTarget )
waitcnt(clkfreq/100 +cnt)
ser.str(string(" I:"))
ser.Dec( ( maxSpeed * 100 ) / I )
waitcnt(clkfreq/100 +cnt)
ser.str(string(" PID:"))
ser.Dec(PID)
waitcnt(clkfreq/100 +cnt)
ser.str(string(" ERR:"))
ser.Dec(error)
ser.tx(13)
'----Set LED to indicate when at position----
if error == 0
outa[LED_PIN] := 1
else
outa[LED_PIN] := 0
PUB VerySimpleControlConcept | x, flp
'THIS METHOD HAS BEEN ADDED TO ORIGINAL OBJECT!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
'all this does is loops forever and increments up and down the "speedTarget" variable connected to the velocity control asm PID loop
x := 0
flp := 1
repeat
repeat x from 0 to 10000 step 100
speedTarget := x * flp
'ser.Dec(currentError)
'ser.tx(13)
waitcnt(clkfreq/100 + cnt)
repeat x from 10000 to 0 step 100
speedTarget := x * flp
'ser.Dec(currentError)
'ser.tx(13)
waitcnt(clkfreq/100 + cnt)
flp *= -1
PUB quadraturePosition(deltaPosition) '' 4 Stack Longs
'' ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
'' // Returns the delta or absolute position of the quadrature encoder in ticks.
'' //
'' // DeltaPosition - True to get the delta position since the last call. False to get the absolute position since last call.
'' ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ifnot(deltaPosition)
return positionMeasurement
deltaPosition := positionMeasurement
result := deltaPosition - deltaPositionMeasurement
deltaPositionMeasurement := deltaPosition
PUB quadratureSpeed(targetSpeed) '' 4 Stack Longs
'' ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
'' // Drives the quadrature encoder at a target speed using proportional, integral, and derivative control.
'' //
'' // TargetSpeed - The target speed in ticks per second. The target speed should be reachable by the system.
'' ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
speedTarget := targetSpeed
PUB PIDEngineStart(motorLeft, motorRight, encoderLeft, encoderRight, PGain, IGain, DGain) '' 13 Stack Longs
'' ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
'' // Starts up the PID driver running on a cog.
'' //
'' // Returns true on success and false on failure.
'' //
'' // MotorLeft - The left drive pin of the h-bridge. -1 to disable output.
'' // MotorRight - The right drive pin of the h-bridge. -1 to disable output.
'' // EncoderLeft - The left leading signal pin of a quadrature encoder. -1 to disable input.
'' // EncoderRight - The right leading signal pin of a quadrature encoder. -1 to disable input.
'' // PGain - Power of two gain for the proportional drive. (0 to 31). Where zero disables the proportional factor.
'' // IGain - Power of two gain for the integral drive. (0 to 31). Where zero disables the integral factor.
'' // DGain - Power of two gain for the derivative drive. (0 to 31). Where zero disables the derivative factor.
'' ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
PIDEngineStop
if(chipver == 1)
leftEncoderPin := ((|<((encoderLeft <# 31) #> 0)) & (encoderLeft <> -1))
rightEncoderPin := ((|<((encoderRight <# 31) #> 0)) & (encoderRight <> -1))
encoderLeft := ((motorLeft <# 31) #> 0)
encoderRight := ((motorRight <# 31) #> 0)
leftDrivePin := ((|<encoderLeft) & (motorLeft <> -1))
rightDrivePin := ((|<encoderRight) & (motorRight <> -1))
leftCounterSetup := (constant(%0_0100 << 26) | encoderLeft)
rightCounterSetup := (constant(%0_0100 << 26) | encoderRight)
proportionalGain := (31 - ((PGain <# 31) #> 0))
integralGain := (31 - ((IGain <# 31) #> 0))
derivativeGain := (31 - ((DGain <# 31) #> 0))
errorAddress := @currentError
positionAddress := @positionMeasurement
cogNumber := cognew(@initialization[0], @speedTarget[0])
result or= ++cogNumber
PUB PIDEngineStop '' 3 Stack Longs
'' ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
'' // Shuts down the PID driver running on a cog.
'' ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
if(cogNumber)
cogstop(-1 + cogNumber~)
DAT
' /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
' PID Driver
' /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
org 0
' //////////////////////Initialization/////////////////////////////////////////////////////////////////////////////////////////
initialization mov frqa, #1 ' Setup counter modules.
mov frqb, #1 '
mov ctra, leftCounterSetup '
mov ctrb, rightCounterSetup '
or dira, leftDrivePin ' Setup outputs.
or dira, rightDrivePin '
mov counter, deltaTime ' Setup sample time.
add counter, cnt '
' /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
' Sync
' /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
loop waitcnt counter, deltaTime ' Wait until sample time.
' //////////////////////Position Input/////////////////////////////////////////////////////////////////////////////////////////
mov buffer, ina ' Sample left and right inputs.
test buffer, leftEncoderPin wc '
test buffer, rightEncoderPin wz '
muxc encoderCurrent, #2 ' Store left and right inputs.
muxnz encoderCurrent, #1 '
initializeOnce mov encoderPrevious, encoderCurrent ' Initializes previous state once.
mov initializeOnce, #0 '
cmp encoderPrevious, encoderCurrent wz ' Update current state.
if_nz rev encoderPrevious, #30 '
if_nz xor encoderPrevious, encoderCurrent '
if_nz cmpsub encoderPrevious, #2 wc, nr '
if_nz sumc encoderPosition, #1 '
wrlong encoderPosition, positionAddress '
mov encoderPrevious, encoderCurrent ' Update previous state.
' //////////////////////Speed Input////////////////////////////////////////////////////////////////////////////////////////////
muxc divideSign, instructionMask ' Flip the sign if negative.
rdlong buffer, #0 ' Check if a second has passed.
cmpsub buffer, speedCounter wc, nr '
if_nz_or_nc mov speedBuffer, speedCounter ' Transfer difference.
if_nz_or_nc mov speedCounter, #0 '
if_z_and_c jmp #divideSkip '
mov averageCounter, speedBuffer ' Setup decay time for average.
add averageCounter, deltaTime '
mov divideBuffer, #0 ' Setup to divide. Buffer has clock frequency.
mov divideCounter, #32 '
divideStart shr speedBuffer, #1 wc, wz ' Start divide.
rcr divideBuffer, #1 '
if_nz djnz divideCounter, #divideStart '
divideFinish cmpsub buffer, divideBuffer wc ' Finish divide.
rcl speedBuffer, #1 '
shr divideBuffer, #1 '
djnz divideCounter, #divideFinish '
divideSign mov speedBuffer, speedBuffer ' Flip the sign if negative.
add averageBuffer, speedBuffer ' Compute average.
sar averageBuffer, #1 '
divideSkip cmp averageCounter, speedCounter wc, wz ' Decay average on slow down.
if_nc_and_nz jmp #divideIncrement '
abs averageBuffer, averageBuffer wc '
cmpsub averageBuffer, #1 '
negc averageBuffer, averageBuffer '
divideIncrement add speedCounter, deltaTime ' Increment ticks.
' //////////////////////Feedback/ AKA /velocity control asm PID///////////////////////////////////////////////////////////////////////////////
rdlong PIDCurrentError, par ' Update current error.
sub PIDCurrentError, averageBuffer '
maxs PIDCurrentError, PIDValueMax '
mins PIDCurrentError, PIDValueMin '
add PIDCounter, PIDCurrentError ' Compute I.
maxs PIDCounter, PIDValueMax '
mins PIDCounter, PIDValueMin '
mov PIDBuffer, PIDCounter '
sar PIDBuffer, integralGain '
mov buffer, PIDCurrentError ' Compute D.
sub buffer, PIDPreviousError '
sar buffer, derivativeGain '
add PIDBuffer, buffer '
mov PIDPreviousError, PIDCurrentError ' Update error.
sar PIDCurrentError, proportionalGain ' Compute P.
add PIDBuffer, PIDCurrentError
wrlong PIDPreviousError, errorAddress
' //////////////////////Output/////////////////////////////////////////////////////////////////////////////////////////////////
neg phsa, PIDBuffer ' Drive forwards or backwards.
mov phsb, PIDBuffer '
' //////////////////////Loop///////////////////////////////////////////////////////////////////////////////////////////////////
jmp #loop ' Repeat.
' /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
' Data
' /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
encoderCurrent long 0
encoderPosition long 0
speedBuffer long 0
speedCounter long 0
averageBuffer long 0
averageCounter long 0
PIDPreviousError long 0
PIDCounter long 0
' //////////////////////Constants//////////////////////////////////////////////////////////////////////////////////////////////
deltaTime long 800
instructionMask long %000001_0000_0000_000000000_000000000
PIDValueMax long 536_870_911
PIDValueMin long -536_870_911
' //////////////////////Configuration Settings/////////////////////////////////////////////////////////////////////////////////
leftCounterSetup long 0
rightCounterSetup long 0
proportionalGain long 0
integralGain long 0
derivativeGain long 0
' //////////////////////Pin Masks//////////////////////////////////////////////////////////////////////////////////////////////
leftEncoderPin long 0
rightEncoderPin long 0
leftDrivePin long 0
rightDrivePin long 0
' //////////////////////Addresses//////////////////////////////////////////////////////////////////////////////////////////////
positionAddress long 0
errorAddress long 0
' //////////////////////Run Time Variables/////////////////////////////////////////////////////////////////////////////////////
buffer res 1
counter res 1
divideBuffer res 1
divideCounter res 1
' //////////////////////Measurement Variables//////////////////////////////////////////////////////////////////////////////////
encoderSpeed res 1
encoderPrevious res 1
' //////////////////////PID Variables//////////////////////////////////////////////////////////////////////////////////////////
PIDCurrentError res 1
PIDBuffer res 1
' /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
fit 496
{{
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// TERMS OF USE: MIT License
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation
// files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy,
// modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all copies or substantial portions of the
// Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
// WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
// COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
// ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
}}
and this is just the isolated algorithm (same as the one in main() from above without the comments)
ifnot posTarget == lastTarget
lastTarget := posTarget
startPosition := positionMeasurement
error := ( posTarget - positionMeasurement )
P := P_Gain * error 'High error gain
I := ( ( || (posTarget - startPosition) - || error ) + D_Gain) * I_Gain #> 1
PID := P / ( ( ( maxSpeed * 100 ) / I ) #> 1 )
PID := PID <# maxSpeed #> maxSpeed*-1
speedTarget := PID
I hope that the way i wrote this algorithm makes sense for someone to understand it, i have never written a PID function and only vaguely understand their inner workings (plus time away from the prop chip has made this a bit of a learning experience), so please try not to laugh!!
for fun i included a pic of the system im building:
And thanks for anyone's help!!
[edit]
I would also like to list some of the non-desirable behavior i noticed in it.
*the acceleration and deceleration of my PID loop seem to be dependent on the value of maxspeed, where larger values = longer accel and decel times.
*when the motor is at rest (holding at position), if the motor's shaft is forcefully rotated and held at some error value and then slowly rotated back to where error=0, the motor will continue to apply force until some "value" is reached in the opposite direction where the loop finally seems to kick in correctly. i think this is considered overshoot? however while debugging i can clearly see the value of both PID and speedTarget in my loop approaching zero and flipping signs properly. so i think this has more to do with the way im updating the parameters in the original Asm based PID loop that controls the velocity of the motor.
1024 x 576 - 64K
Comments
I am curious, was this part of the original? How can the output ever reach 0? In this equation, it looks like you will have many cases of P being divided by a larger number.
You will need the target position, and the current encoder position to derive the error. At a glance I am not sure how you' is working. Are the encoders zero'd out on bootup at a home position, such that the range must always be positive?
In fact 90% of velocity or temperature or fill PID applications I only used P & I with great success. I simply start with P raise it until I get over shoot and back it down a bit then add I-gain until I reach SP. This usually worked even on systems which may have huge variations of the mass being controlled like critical tension/speed of large rolls of paper into a printing press were you start off with a ton or so of paper but need to control it until there is almost nothing left.
In all honesty the only times I used D-gain was on a system where the running velocity could be changed by the operator prior to the system starting and I needed more or less gain from zero to running speed. I would pre-populate the D-gain based upon operator entered run speeds.