PID-filter routine (2nd try).

[HenrikOlsson]

Hi,
(Ok, lets try this again. The previous version contained a non working example which is not very suitable in the Example Section so it has been updated. Here goes...)

This is a basic PID filter routine in the form of an include file.

Instructions:
1) Download the file incPID.txt attached to this post
2) Rename it to incPID.pbp
3) Place it in your project folder.

Example code:
This is a short example demonstrating how the PID routine can be used. The PIC's CCP module drives a motor via a suitable driver chip such as the LMD 18200. The motor shaft is connected to a potentiometer which is feeding position information back to the PIC's ADC forming a closed loop.

' |--PIC CCP --> LMD18200 --> Motor --> Potentiomter --> PIC ADC --|
' |                                                                |
' |---<-----<-----<------ P I D  F i l t e r <------<------<-------| 



  '// Set up hardware registers, ADC etc here (not shown) //

  ADValue VAR WORD                     '<---This is your variable.
  SetPoint VAR WORD                    '<---This is your variable.
  Direction var PortB.7                '<---Direction bit to motor driver.

  Setpoint = 512		       '<---Set desired position.

  INCLUDE "incPID.pbp"                 'Include the PID routine.

    'These variables are declared by the incPID routine but
    'the user needs to assign values to them.
    pid_Kp = $0700                     'Set Kp to 7.0
    pid_Ki = $0080                     'Set Ki to 0.5
    pid_Kd = $0225                     'Set Kd to 2.14
    pid_Ti = 8                         'Update I-term every 8th call to PID
    pid_I_Clamp = 100                  'Clamp I-term to max ±100
    pid_Out_Clamp = 511                'Clamp the final output to ±511
     
  Start:
    Gosub GetAD                        'Get position from AD - NOT SHOWN HERE.
                                     
    pid_Error = Setpoint - ADValue     'Calculate the error
    Gosub PID                          'Result returned in pid_Drive
    Direction = pid_Out.15             'Set direction pin accordning to sign
    pid_Out = ABS pid_Out              'Convert from two's comp. to absolute
    HPWM 1, pid_Out, 10000             'Set PWM output
    Pause 10                           'Wait....
  Goto Start                           '...and do it again.

The code does not check for overflow in the calculations so the user needs to make sure that the value sent to the PID routine doesn't cause an overflow based on the current P, I and D gains.

More detalied information can be found in the attached file.

A special thank you goes to Darrel Taylor for spotting a couple of misstakes in the example but most of all for streamlining the code further, reducing its footprint about 20%.

/Henrik Olsson.

[RodSTAR]

hi Henrik;
i see you are a motor control specialist..
first of all thank you for sharing your work on your PID routine. About it i have some questions to understand.
_________
question1:
i don't understand theese variables:
pid_I_Clamp = 100 'Clamp I-term to max ±100
pid_Out_Clamp = 511 'Clamp the final output to ±511

_________
question2:
I see it controls position. ¿Does it control speed?

_________
question3:
from my pc i get two parameters: position and speed. i want to preset a 1 second of gradual speed increasing (you know, for not damaging mechanism & stuff..) and speed decreasing for before getting to the given position. Do any of your PID inc file variables handle this parameter/s or i need to do it by myself?

to let you breathe, it's all for now thank you.

regards, Rodrigo

[HenrikOlsson]

Hi Rodrigo,
No, no motorcontrol specialist at all actually, just a guy who tinkers with a lot of stuff, trying to pick up on what ever interests me at the moment ;-)

question1:
i don't understand theese variables:
pid_I_Clamp = 100 'Clamp I-term to max ±100
pid_Out_Clamp = 511 'Clamp the final output to ±511

The pid_I_clamp helps preventing what is called integrator wind-up. Let's say you are controlling the position of a motor. You command a new position but the mechanics that the motor is moving is blocked so it can't move. The I-term will now start to add "effort" to the total output of the filter to get the motor moving (decrease the error) but since the mechanics is blocked the error won't change so the I-term gets larger and larger and larger. When the mechancis finally "loosens up" you can imagine what happens. So with pid_I_clamp you can set the max amount of "effort" that the I-term will apply to the total output, no matter how big or how long the error persists.

(Another thing is that the I-term output is not protected from overflowing internally so we have to clamp it to something)

pid_Out_Clamp works the same way. Let's say you are running a 12V motor with a 24V powersupply. Applying full power (24V) to the motor may damage it so you can use the pid_Out_Clamp to prevent the final output of the PID-filter to ever go above a certain value, for example.

question2:
I see it controls position. ¿Does it control speed?

"It" controls anything you like. You provide it with an "error" and gives back a "drive-value" based on the gains etc that you've specified. It doesn't know or care what it is you are controling. With that being said it may lack features that is better suited for controling one thing or another, like feedforward etc but more or less, it doesn't care.

question3:
from my pc i get two parameters: position and speed. i want to preset a 1 second of gradual speed increasing (you know, for not damaging mechanism & stuff..) and speed decreasing for before getting to the given position. Do any of your PID inc file variables handle this parameter/s or i need to do it by myself?

No, there's no trajectory planner built in. If you want acceleration and deceleration ramps you need to "slowly" increase the "setpoint" to bring it up to speed and so on.

Hope that answers your questions.

/Henrik.

[RodSTAR]

Thank you so much Henrik, now I got it!
I'll test it as soon as I can. Then i'll bother you again

I have another question in a new thread, not related to PID, but related to counting position. Thread title: "Pulse count matching"

[infinitrix]

can i use the code to control speed and position of motor using same pic?

[RodSTAR]

can i use the code to control speed and position of motor using same pic?

First step (get), you Get variables from hardware (via pot, encoder, ...)
Second step (control), you correct those variables (PID between desired and got variables)) to reach your desired speed, direction & position by PWM and polarity (depending on the type of motor DC, BLDC or what ever). That is control.

And yes, both steps you can perform them in a single MCU. Motor control oriented MCU's works great for this, such as a dsPIC30F3011; or for this compiler, a PIC18F2431 (or any of the family PIC18Fxx31) which have QEI peripheral, where QEI stands for Quadrature Encoder Interface, which decodes signal from a rotary Quadrature Encoder, ideal for gathering motor rotation activity.

Rodrigo M.-

[infinitrix]

i'm using 1 channel incremental encoder and PIC16F877A.i use timer 1 to count pulse on background.to get the speed i use "count" instruction but it take a 0.5 second.is there any faster solution to calculate speed?

i decide to use incPID program to control speed,but how can i tune to find Kp,Ki, and Kd value..

[infinitrix]

anyone know how to tune PID coefficient? help me..

[amgen]

.5 sec is too long to get speed reading, you may be able to timer/counter 1 pulse duration to get speed ref faster .
PID itself is hard to tune. Isn't each correction just how-much and how fast ? and slowing-down and reducing each correction until there is no overshoot ? I made an alternator/regulator controller that self-tuned by jointly reducing correction amount and slowing corrections (longer pauses) until there were no more overshoots. The output was the PWM to MOSFET. You could also adjust up/down the gain/time with a pot input ref.
Are you interested in PID discussion ?
don
amgen

[infinitrix]

i'm interested in pid to control position and speed.. can u show me some program and schematic for self tune pid.. i use 3 pot to adjust kp,ki and kd.. i'm just not very clear about tuning the pid..

[amgen]

do you need to move something to a position and stop, or shift motor from 1 speed to another speed or hold speed constant while the load changes.
That dictates the type of feedback needed and how fast the control needs to actuate.
Explain your control operation a little.

don
amgen

[HenrikOlsson]

Hello,
Sorry for the slow response on my behalf, I've been on the road so I haven't been on the 'net that much.

As I've written en previous posts in this thread the incPID routine itself does not know or care if it controls position, speed, temperature, waterlevel, light, torque or whatever. It simply returns a value based on the "error" you send it and the tuning parameters you set.

You can only run one PID-loop with it though (without rewriting the include) so controling BOTH speed AND position with the incPID won't work. What you CAN do though is to set up your system to control position and then in your program you continously change the targetposition. Set up a 100Hz (or whatever) interrupt and add to or subtract from the target position, the more you add or subtract the faster it will go. I've done that myself and it works fairly well.

Tuning is an art and there's more than a few ways to do it, Google Ziegler Nichols to read up on one of them. I've simply done it by "feel" but you can for example send the errror value to the USART and then plot the value on the PC using EXCEL or whatever to "see" the response of the system.

I suppose doing an autotune routine would be possible but it's not something I'm going to try. If you're going to write an autotune routine I suggest you read up and get good at manually tuning first so you know and understand how to respond to different situations. (Undershoot, overshoot, oscillation etc).

Hope it helps!
/Henrik.

[hadiengg]

Dear sir,
I check INCPID routine it works fine as position control.
I want to control speed via PWM what should i change in the INCPID routine
and also how can i give pid_Kp pid_Ki pid_Kd using external potantiometer
which is reading through ADC CH which gives 0-1023 value and how i convert
this value to $0xxx figure.

[RodSTAR]

Dear sir,
I check INCPID routine it works fine as position control.
I want to control speed via PWM what should i change in the INCPID routine
and also how can i give pid_Kp pid_Ki pid_Kd using external potantiometer
which is reading through ADC CH which gives 0-1023 value and how i convert
this value to $0xxx figure.

As I understand, this PID checks only position.
To control speed, you should use another routine to control position AMONG TIME (speed), using this PID routine as position control. I did that and works fine. Not just simple speed but parametrical trapezoidal (lineal, curve, log, exp) starts and ends, via USB to my laptop. It works fine almost like an industrial servomotor (24V 500W DC motors). My next experiment is to implement it with 4 BLDC motors I have. I'm in the way to control with a iPhone. Henrik doesn't know how gratefull I am to him for his seed piece.

[hadiengg]

Dear sir,
What should i change in INCPID file that it controls the speed of motor not position.
And also i want to put Kp Ki Kd values through potantiometers.How it is possible.?

[HenrikOlsson]

Hi,
You don't need to change anything. If you're aiming for speed control only then simply measure the speed of your motor with whatever you like, encoder, tachometer, BEMF or whatever. Compare that to your target speed and send the difference between the two (ie. the error) to the PID-filter. The filter itself does not know or care what you are regulating, it might as well have been air-pressure as far as the PID-routine is concerned.

If you need to move to a certain position WITH a certain speed then you need to take the moving target aproach like RODStart outlined. If you control position and add 1 to the target position every second the motor will move 1 encoder count every second or 10.8° per minute or 0.03rpm with a 500line encoder.

Regarding the pots, you can set the PID gains in any number system you like. The reason I show it in hex is that it is easier IMO. You can simply do pid_Kp = myADResult if you like.

1023 in decimal is the same as $03FF which, in the PID filter, is treated as 3.996 basically. See, 03 is the number on the left side of the decimalpoint and FF is on the right side. FF represents 1/256*255 or 0.996. If you'd feed it 102 from the ADConverter it would be the same as $0066 or a gain of 0.258.

If you need more gain than 1023 then simply multiply your ADResult, pid_KP=myADResult*2, now you get a gain of 0-2046 or $0000-$07FE or 0.0-7.992

/Henrik.

[hadiengg]

Dear sir thank u for reply.
My question is that i can get 10 bit resolution of PWM output to control motor speed.
I use following code.
CCP1CON = %00001100 ' Set CCP1 to PWM
T2CON = %00000111 ' Turn on Timer2, Prescale=4
PR2 = 249 ' Set PR2 to get 1KHz out
CCP1CON.4 = pid_out.0 ' Store duty to registers as
CCP1CON.5 = pid_out.1 ' a 10-bit word
CCPR1L = pid_out >> 2
It will give 10bit PWM output at 16MHz crystal.
Now spose pid_Error=+5 then spose after calculation
pid_Out=100
pid_out.15=0 'DIRECTION BIT
same result will be if pid_Error=-5 but
pid_out.15=1
With both errors final PWM resolution is same but how can I use direction bit to change PWM output to increase or decrease motor speed according to requirement.?
If there is any OFFSET which must be used ?

[HenrikOlsson]

Hi,
The approach with the direction bit is for sign and magnitude PWM while, by the sound of it, you're looking for locked anti phase. In locked antiphase the PWM dutycycle is 50% when no torque should be delivered by the motor (the average current thru the motor is 0A). The dutycycle is then increased or decreased from 50% depending on which direction and how much torque the motor should deliver.

So, if you ARE doing locked antiphase PWM simply set the PWM duty cycle to 50% on startup and then, if pid_out.15 is set (output is negative) you subtract the ABS value of pid_out from the value coresponding to 50% and if pid-out.15 is "0" you add the ABS value of pid_out to value coresponding to 50%.

If you are doing sign and magnitude then your hardware will have an input for direction and you need to set that according to pid_out.15 and then set the dutycycle to the ABS value of pid_out.

I sugest you set the pid_Out_Clamp variable to a suitable value to prevent over or underflow in the duty-cycle register.

/Henrik.

[hadiengg]

Thanks for coperation.
I am trying to complete my job.

[daan.joubert]

Hi Henrik,
Is it possible to use portion of the code on a RC servo which is connected to an externat 10k potentiometer.?
My existing circuit is driving an RC servo in the range of 1mS to 2mS, the servo pulls a throtle of an engine, to keep revolutions at 1500 RPM (50Hz). The engine is directly coupled to an alternator. The position of the servo is controlled by a voltage (F/V), form the alternator frequency and a 10k setpoint potentiometer. I use analog inputs on 16f88 to convert volt to value. I then subtract values from each other and send the difference to the servo. (simple calculation)
My problem is that the voltage conversion is not linear to the servo movement. The result is that servo movement is always too litlle in comparison of the set speed needed.
I was wondering whether I can use feedback in order to counteract the non-linear characteristic of the servo.

[Acetronics2]

Hi, Daaan

I use such a routine to drive cooling flaps of an engine cowl ... ( of course with temp. as input ... lol )

no problems just change the lines :

    Direction = pid_Out.15             'Set direction pin accordning to sign
    pid_Out = ABS pid_Out              'Convert from two's comp. to absolute
    HPWM 1, pid_Out, 10000             'Set PWM output
    Pause 10                           'Wait....

to drive a servo and not a " Bridge driven " DC motor.

Did not understand your linearity problems ... IF you use the servo within its NORMAL RANGE ( say 800 - 2200 µs ) linearity is fine - Whatever its brand ( humour !!! )

Moreover, ... as it's a closed control loop with Integral action, your servo HAS to reach the required position ... more or less qickly !!!

But, might be, your project is only a PROPORTIONNAL regulator ... This could easily explain never reaching the setpoint ...

Alain

[anonymouse]

Hi Henrik,
Is it possible to use portion of the code on a RC servo which is connected to an externat 10k potentiometer.?
My existing circuit is driving an RC servo in the range of 1mS to 2mS, the servo pulls a throtle of an engine, to keep revolutions at 1500 RPM (50Hz). The engine is directly coupled to an alternator. The position of the servo is controlled by a voltage (F/V), form the alternator frequency and a 10k setpoint potentiometer. I use analog inputs on 16f88 to convert volt to value. I then subtract values from each other and send the difference to the servo. (simple calculation)
My problem is that the voltage conversion is not linear to the servo movement. The result is that servo movement is always too litlle in comparison of the set speed needed.
I was wondering whether I can use feedback in order to counteract the non-linear characteristic of the servo.

I have used a magnetic pickup on the fly wheel in the past, adc this to a stepper motor, incorporate the feedback once you get to the stage of engine hunting, then set your fine tuning (gain etc).

[hadiengg]

I have many trys to solve my problem but fail.Actually i want to control GAS engine speed.I am using AGB130 LINEAR ELECTRIC ACTUATOR to drive BUTTERFLY VALVE.I am using MAGNETIC PICKUP UNIT located on flyweel to sense engine speed.After F/V conversion i have 0-5000Hz to 0-5V.
Problem is that ACTUATOR drive only forward direction through given voltage
and reverse direction through SPRING located inside.
ACTUATOR always needed some voltage to remain in a position to maintain speed.But INCPID routine gives 0 output when no error found.
I am giving some examples in atachements which will help to identify problem.
Best regards
IFTIKHAR AHMAD

[HenrikOlsson]

Hi,
Don't really know how the spring return will work here...Can't really see why it wouldn't work....

The 0V at 0 error isn't really true.... When the error is 0 output from the P- and D-term is indeed 0 (the D-term might not be 0 but for this discussion let's say it is). But the output from the I-term is whatever was needed to bring the error to 0. (It might be 0 but not necessarily).

Think of the car analogy, you're trying to keep the speed at 50mph and you're starting to climb a steep hill, if you simply keep the gas-pedal at the exact same position the speed will start to drop. The P-term adds a little effort based on the error but not enough since the load now is larger (we're climbing a hill). The I-term starts adding a little effort (giving some gas) and soon the velocity is back at 50mph. Now the P term is 0 (since there is no error) the D-term is 0 since there is no error but the I-term is not zero - if it were the speed would start to drop again.

With that said, simply add a little to the output of the PID-filter rto overcome the force of the spring - if that's what you want. This is often called feed-forward and means that for a particular speed, torque, position or whatever you have like a "starting point" for the "drive" - the PID filter then takes it from there.

/Henrik.

[RodSTAR]

I have many trys to solve my problem but fail.Actually i want to control GAS engine speed.I am using AGB130 LINEAR ELECTRIC ACTUATOR to drive BUTTERFLY VALVE.I am using MAGNETIC PICKUP UNIT located on flyweel to sense engine speed.After F/V conversion i have 0-5000Hz to 0-5V.
Problem is that ACTUATOR drive only forward direction through given voltage
and reverse direction through SPRING located inside.
ACTUATOR always needed some voltage to remain in a position to maintain speed.But INCPID routine gives 0 output when no error found.
I am giving some examples in atachements which will help to identify problem.
Best regards
IFTIKHAR AHMAD

PID is a loop. If spring moves something, PID will detect it and it will correct it immediatly. Don't confuse error variable with PWM value. In my project it works fine and it responds to dynamic loads and it correct it as it should (read above post of my project). Sorry for answering you, as you only refer to Henrik.

[anonymouse]

I have many trys to solve my problem but fail.Actually i want to control GAS engine speed.I am using AGB130 LINEAR ELECTRIC ACTUATOR to drive BUTTERFLY VALVE.I am using MAGNETIC PICKUP UNIT located on flyweel to sense engine speed.After F/V conversion i have 0-5000Hz to 0-5V.
Problem is that ACTUATOR drive only forward direction through given voltage
and reverse direction through SPRING located inside.
ACTUATOR always needed some voltage to remain in a position to maintain speed.But INCPID routine gives 0 output when no error found.
I am giving some examples in atachements which will help to identify problem.
Best regards
IFTIKHAR AHMAD

You will need to find the required pwm to output 1500 rpm (1.5v) from your engine,(you will need this on startup to have your engine idling at no load) this you will use as your setpoint after adc. Anything above it will generate a negative P substracting from setpoint,slowing the engine down, above it adding to the setpoint,speeding up the engine. The gains of each part of p.i.d will be from the variable resistors via adcs. Have these initially set to one, set the p then p gain etc the idea is to have the engine fairly responsive to load change and the fine settings of the gains will limit the droop and overshoot.

[daan.joubert]

Hi Alain and others,
Thank you all for info given, it is much appreciated.

Alain, regarding the linear problem - I have tested the actual distance travel of the servo arm by connecting a ruler to the arm.
To test I then adjusted the speed voltage, taking voltage readings and distance readings as I go along.
The distance values on Excel graph showed a non linear line in comparison to the linear nature of the voltage.
/Daan Joubert

[Acetronics2]

Hi Alain

The distance values on Excel graph showed a non linear line in comparison to the linear nature of the voltage.
/Daan Joubert

Hi, Daan

I hope you've considered the rotation angle of the arm is linear to the signal ...

But, whatever your transmission system ... only rack and pinion or cable and pulley can give a linear travel !!!

You could use a LOOKUP Table to correct your output ... but, once more, if it is a real PID controller, it will compensate for non-linearities ...

Or try to use ... FUZZY LOGIC !!!

Or find an old LINEAR R/C servo ( Kraft, Robbe, Varioprop, Lextronic ... produced some in the 70-80's )


Alain

[anonymouse]

Hi Alain and others,
Thank you all for info given, it is much appreciated.

Alain, regarding the linear problem - I have tested the actual distance travel of the servo arm by connecting a ruler to the arm.
To test I then adjusted the speed voltage, taking voltage readings and distance readings as I go along.
The distance values on Excel graph showed a non linear line in comparison to the linear nature of the voltage.
/Daan Joubert

Your gains might need to be fractional all the way up to 20 times and more via your vresistors via adc. Example engine running no load 1500 rpm, adc 1.5/5 * 256 = 77. setpoint = 77,unknown but say pwm 25, all gains apart from p set to zero. 25% load applied engine droops to 500rpm,adc 0.5/5 * 256 = 26. 77-26= 51, pwm output 51. If output to much p gain * 0.8 etc,to little * 1.5 etc. You should get to the stage of engine hunting turn the gain slightly below this. This is then where the data from i and d comes into play and slightly tweaking each one results in a reactive genset regardless of load.

[hadiengg]

Thanks Mr Henrik,Mr RodSTAR and Mr Anonymouse for help.
I succesfully run the motor at constant speed.Motor speed also change through given speed refrence.
Now one problem that i face is motor overshoot in slow speed refrence whenever i turn on the power.Its overshoot cycle comes down when i increase speed refrence.At full speed refrence it get exact speed never overshoot.I am using same PID gains used in original file.I think i must change the GAIN value to overcome this problem.
And also i am using variable for 0-5V to 0-1023 value through ADC.How i use this value to put the gain as used under.
pid_Kp = $0700 'Set Kp to 7.0
Please give some code to convert the value.

[HenrikOlsson]

Hello,
I thought I explained that in post #16 above. I'll simply quote myself:

Regarding the pots, you can set the PID gains in any number system you like. The reason I show it in hex is that it is easier IMO. You can simply do pid_Kp = myADResult if you like.

1023 in decimal is the same as $03FF which, in the PID filter, is treated as 3.996 basically. See, 03 is the number on the left side of the decimalpoint and FF is on the right side. FF represents 1/256*255 or 0.996. If you'd feed it 102 from the ADConverter it would be the same as $0066 or a gain of 0.258.

If you need more gain than 1023 then simply multiply your ADResult, pid_KP=myADResult*2, now you get a gain of 0-2046 or $0000-$07FE or 0.0-7.992

I don't really know how to explain it in any other way but let me know if doesn't make sense and I'll try, or perhaps someone else can attack it from another angle.

/Henrik.

[Acetronics2]

Hi, Hadienggggggg

At full speed refrence it get exact speed never overshoot.I am using same PID gains used in original file.I think i must change the GAIN value to overcome this problem

EACH system has its own set of parameters ( Kp, Ki, Kd ) and you are the one and only one who can find them ...

NOW, just Google around for the " Ziegler and Nichols method " ... sure it will help you !

FURTHER ... you can disable The Ki and Kd action at startup and engage then for a maximum error of ???, you can use retrieve tables for Kp,Ki,Kd as a function of the running point, you can program a rampup for engine start ...

But that is VERY typical to YOUR project ... and far out from Henrik's project shown here ...

BTW ... MERCI Henrik ...

Alain

[phoenix_1]

The QEI reading work good with count for position from 0-1000.
The PPWM give good voltage and signal too from 0 - 1024 at 20KHz.
The Henrik PID rutine work good but only when position and setpoint is max +/-50 !
and realy how make to motor clip +/- when position = setposition in some range about +/- 25% as break in position.
My output driver have next logic :
with PWM 512 motor STOP
0<--------512----->1024
left <---- stop-----> right
fast slow stop slow fast

Here is my debug program:

'****************************************************************
'                 Definicije PIC-a  18F4431                     *
'****************************************************************
    DEFINE OSC 20
    include "incPID.pbp"
'****************************************************************
'                        LCD DEFINE LCD 4X20 chr             *
'****************************************************************
    DEFINE LCD_DREG PORTD
    DEFINE LCD_DBIT 4
    DEFINE LCD_RSREG PORTC
    DEFINE LCD_RSBIT 0
    DEFINE LCD_EREG PORTC
    DEFINE LCD_EBIT 1
    DEFINE LCD_BITS 4
    DEFINE LCD_LINES 4
    DEFINE LCD_COMMANDUS 2000
    DEFINE LCD_DATAUS 50
                 
'****************************************************************
'                          Definicije Portova                   *
'****************************************************************    
    ADCON0=0                                
    ANSEL0=0
    TRISB = %00000000
    PortA = 0                               
    PortB = 0                                  
    PortC = 0
    PortD = 0                                
    PortC.3 = 1                                 
                         
'****************************************************************
'                          Definicije  PPWM                     *
'****************************************************************   
    DTCON = 0 
    PTCON0 = %00000000 
    PTCON1 = %10000000 
    PTPERL=$FF      
    PTPERH=$00
    PWMCON0 = %00100000 
    PWMCON1 = 1 
    OVDCOND = %11111111                     
'****************************************************************
'                          Definicije QEI encodera              *
'**************************************************************** 
    'QEICON = %10011000
    'DFLTCON = %00111000                
    'MAXCNTL = %11001111              
    'MAXCNTH = %00000111
    QEICON=%10001000
    DFLTCON = %00111000                
    MAXCNTL = %11101000               
    MAXCNTH = %00000011               
    PosLow VAR BYTE                           
    PosHigh VAR BYTE                           
    PosTemp VAR BYTE                          
    Position VAR WORD                          
    POSCNTL = 0                                
    POSCNTH = 0
'****************************************************************
'                          Definicije  TMR0                     *
'****************************************************************     
    T0CON = %11001000
    INTCON.5 = 1
    INTCON.1 = 0
    INTCON.7 = 1
    INTCON.4 = 0
    INTCON.3 = 0
    INTCON.2 = 0
    INTCON2.2 = 1
    RCON.7 = 1
    TMR0L =254 
'****************************************************************
'                          Definicije PID filtera               *
'****************************************************************
    x var word
    y var word
    ADValue VAR word                           '<---This is your variable.
    Setpoint VAR WORD                          '<---This is your variable.
    Direction var bit                          '<---Direction bit to motor driver 
    pid_Kp = $0700                             'Set Kp to 7.0
    pid_Ki = $0080                             'Set Ki to 0.5
    pid_Kd = $0225                             'Set Kd to 2.14
    pid_Ti = 8                                 'Update I-term every 8th call to PID
    pid_I_Clamp = 100                          'Clamp I-term to max ±100
    pid_Out_Clamp = 511                        'Clamp the final output to ±511
    x = 0
    y = 0
    setpoint = 100                               
    pause 250                                  
    lcdout $fe,1                               
'****************************************************************
'                          Osnovna petlja                       *
'****************************************************************
on interrupt goto PIDcalc
     
     loop:
     if portc.3 = 1 then setpoint = setpoint + 1
     if setpoint > 1000 then setpoint = 0
     LCDOUT $fe,128,"POSITION = ",dec position  
     LCDOUT $fe,192,"SETPOINT = ",dec setpoint  
     LCDOUT $fe,148,"PID = ",dec pid_out        
     LCDOUT $fe,212,"PWM = ",dec y              
     pause 10                                   
     lcdout $fe,1                               
     goto loop                                  
         
     disable
PIDcalc:
     INTCON.1 = 0                                
     if INTCON.2 = 1 then                        
     PosHigh = POSCNTH                           
     PosLow = POSCNTL                            
     PosTemp = POSCNTL                           
     If PosLow - PosTemp = 0 then Goto Done      
     PosHigh = POSCNTH                           
     PosLow = POSCNTL
Done: 
     Position = POSHIGH * 256 + PosLow           
     advalue = position 
     pid_Error = Setpoint - ADValue               
     Gosub PID                                    
     Direction = pid_Out.15                       
     pid_out = ABS pid_Out
     x = (512 - pid_Out) + 512
     select case direction
     case 1
     y = pid_Out
     case 0
     y = x
     end select
     PDC0L = y.LowByte
     PDC0H = y.HighByte
     endif
     INTCON.2 = 0                                 
     resume                                       
     enable
     end

If any help pls.
Regards Robert

[HenrikOlsson]

The Henrik PID rutine work good but only when position and setpoint is max +/-50 !

OK, so what does it do if the setpoint is 51 then?

I see you're using On Interrupt and it seems you're using TMR0 as the source for the interrupt. It would be good if you could comment that section a bit so we don't have to read thru the datasheet in order to figure out what you're trying to do.

If I haven't missed anything you're running TMR0 in 8bit mode, clocked from the internal clock @20/4=5Mhz, prescaler selection is set to 1:2 which means the timer is clocked at 2.5Mhz, overflowing and generating interrupts at 9765.625Hz. There's no need to run the PID-loop at that speed, generally 1000Hz is good enough for motor control.

The Pause 10 statement needs to go away, and the LCDOut statements will likely mess with the timing as well. Read up on how On Interrupt works in the PBP manual. I highly recomend you look up Darrels Instant Interrupt routines as they don't suffer from the things On Interrupt does.

What do you mean by

and realy how make to motor clip +/- when position = setposition in some range about +/- 25% as break in position

It's really hard to understnad what you mean..... A deadband where anything within 25% of the setpoint is good enough or a following error trip and when the error is bigger than a set amount?

I've just received my copy of v2.60 so I can start using 32bit variables. I'm having a bt of a problem getting it going though. On top of that I fried my servo development board by connecting 24V to the 5V input so I need to get that sorted....

[phoenix_1]

here is what I rewrite in incPID

'*******************************************************************************
'*******************************************************************************
'Calculate the total drive.

pid_Out = pid_P + pid_I + pid_D         'Calculate total drive....

 pid_Sign = pid_Out.15                   'Save Sign
 pid_Out = ABS pid_Out                   'Convert from two's comp. to abs.
'if  pid_Out >= pid_Out_Clamp then        'Output is saturated...
if  pid_Out >= 640 then
    pid_Status_Out_Sat = 1              'set status bit and...
     pid_out = pid_Out_Clamp             'clamp output.

Endif
'If pid_Sign then pid_out = -pid_out     'Re-apply sign.

RETURN                                  'And return to sender.
SkipPID:

now the PID work close to good , ABS pid_error is in range close to 511 +/- 100 when I probe to personal move shaft of motor.
Becous my motor have gear 6.25:1 my variable ( y as LONG) go from 0-6250 for one full turn of shaft.Motor must go 6.25 * 1000.
QEI is setup for one turn of motor shaft go 0-1000.
It is not full good but it is close to be PID :-(
PBP code :

'****************************************************************
'                 Definicije PIC-a  18F4431                     *
'****************************************************************
    DEFINE OSC 20
    include "incPID.pbp"
'****************************************************************
'                         RS 232 HSEROUT 19200 kbps             *
'****************************************************************
    DEFINE LCD_DREG PORTD
    DEFINE LCD_DBIT 4
    DEFINE LCD_RSREG PORTC
    DEFINE LCD_RSBIT 0
    DEFINE LCD_EREG PORTC
    DEFINE LCD_EBIT 1
    DEFINE LCD_BITS 4
    DEFINE LCD_LINES 4
    DEFINE LCD_COMMANDUS 2000
    DEFINE LCD_DATAUS 50
                 
'****************************************************************
'                          Definicije Portova                   *
'****************************************************************    
    ADCON0=0                                
    ANSEL0=0
    TRISB = %00000000
    PortA = 0                               
    PortB = 0                                  
    PortC = 0
    PortD = 0                                
    PortC.3 = 1                                 
                         
'****************************************************************
'                          Definicije  PPWM                     *
'****************************************************************   
    DTCON = 0 
    PTCON0 = %00000000 
    PTCON1 = %10000000 
    PTPERL=$FF      
    PTPERH=$00
    PWMCON0 = %00100000 
    PWMCON1 = 1 
    OVDCOND = %11111111                     
'****************************************************************
'                          Definicije QEI encodera              *
'**************************************************************** 
    QEICON=%10001000
    DFLTCON = %00111000                ' enable error filter for all capture inputs
    MAXCNTL = %11101000               'low set for maxcnt of 12500 becouse have gear 6.25:1
    MAXCNTH = %00000011                'hig set for maxcnt of 12500 becouse have gear 6.25:1             
    PosLow VAR BYTE                           
    PosHigh VAR BYTE                           
    PosTemp VAR BYTE                          
    Position VAR WORD  
    POSCNTL = 0                        ' clear lowbyte of counter for start
    POSCNTH = 0                        ' clear highbyte of counter for start                         
'****************************************************************
'                          Definicije  TMR0                     *
'****************************************************************     
    T0CON = %11001111
    INTCON.5 = 1
    INTCON.1 = 0
    INTCON.7 = 1
    INTCON.4 = 0
    INTCON.3 = 0
    INTCON.2 = 0
    INTCON2.2 = 1
    RCON.7 = 1
    TMR0L =237       ' 8bit ,1:256 divider ,237 preset = 1000HZ at 20MHz clk
'****************************************************************
'                          Definicije PID filtera               *
'****************************************************************
    s var word
    f var word
    y var long                         'Total pulse count storage variable 32bit
    x var byte                         'Number of full shaft rotation of 360'
    z var BIT                          'Actual bit if any move of shaft to any direction
    h var BIT                          'direction bit 0 for left , 1 for right turn of encoder
    ADValue VAR word                           '<---This is your variable.
    Setpoint VAR WORD                          '<---This is your variable.
    Direction var bit                          '<---Direction bit to motor driver 
    pid_Kp = $0700                             'Set Kp to 7.0
    pid_Ki = $0080                             'Set Ki to 0.5
    pid_Kd = $0225                             'Set Kd to 2.14
    pid_Ti = 8                                 'Update I-term every 8th call to PID
    pid_I_Clamp = 100                         'Clamp I-term to max ±100
    pid_Out_Clamp = 511                       'Clamp the final output to ±511
    s = 0
    f = 0    
    z = 0                                 
    x = 0 
    y = 0
    h = 0
    PIR3.2 = 0 
    setpoint = 0        
    pause 250                                  
    lcdout $fe,1                               
'****************************************************************
'                          Osnovna petlja                       *
'****************************************************************
on interrupt goto PIDcalc
    
     loop:
     if portc.3 = 1 then setpoint = setpoint + 1
     if setpoint > 6250 then setpoint = 0
     LCDOUT $fe,128,"POSITION = ",dec y  
     LCDOUT $fe,192,"SETPOINT = ",dec setpoint  
     LCDOUT $fe,148,"PID = ",dec pid_out        
     LCDOUT $fe,212,"PWM = ",dec f," PE = ",dec ABS pid_error
     LCDOUT $fe,128,"POSITION =     "     'clear line without use $fe,1
     LCDOUT $fe,192,"SETPOINT =     "     'clear line without use $fe,1

     goto loop                                  
         
     disable
PIDcalc:
     INTCON.1 = 0                                
     if INTCON.2 = 1 then                        
     z = 0
     PosHigh = POSCNTH                           'Get high byte of counter
     PosLow = POSCNTL                            'Get low byte of counter
     PosTemp = POSCNTL                           'Get low byte again  
     If PosLow - PosTemp = 0 then Goto Done      'Compare, if equal we're done.
     PosHigh = POSCNTH                           'If not, get values again.
     PosLow = POSCNTL
     z = 1 
Done: 
     Position = POSHIGH * 256 + PosLow           'Put high and lowbyte together. 
     h = QEICON.5                                'QEICON.5 for right = 1 for left = 0 
     if PIR3.2 = 1 and h =1 then x = x+ 1
     if x <> 0 and PIR3.2 = 1 and h =0 then x = x- 1
     PIR3.2 = 0                                  'PIR3.2 maxcount transition down or up was detected
     h = 0 
     y =  (1000 * x) + position                    ' increment  total counter
     if z = 1 and h = 0 then y = y -(1000-position) ' dectement  total counter 
     if y > 6250 then
     x = 0
     endif
     advalue = y
     pid_Error = Setpoint - ADValue
     gosub PID                                    
     Direction = pid_Out.15                       
     pid_out = ABS pid_Out
     s = (512 - pid_Out) + 512
     select case direction
     case 0
     f = pid_Out
     case 1
     f = s
     end select
     PDC0L = f.LowByte
     PDC0H = f.HighByte
     endif 
     INTCON.2 = 0                                 
     resume                                       
     enable
     end

[HenrikOlsson]

Hi Robert,
I'm sorry but I'm having a hard time understanding you. I like to help you but it's just hard when you don't even answer my questions. Now you say it's close to being PID? What exactly is it that doesn't work?

If the motor isn't "stiff" enough you need to tune the PID parameters, I see you have Kp, Ki & Kd exactly as in my example code - which is just that - an example. You need to change them to suit your application.

Also, you could as well changed your code to read pid_Out_Clamp = 640 instead of hardcoding 640 into the incPID routine. In any case if you only have 10bits of PWM resolution why allow the output to swing to +/-640 which is 11bits?

Another thing I notice is that you preset TMR 0 with the value 237, but you do that only at the beginning of the program, so it will go from 237->256, cause an interrupt, then it will start from 0 again. With the pescaler at 1:256 that will be an interrupt rate of 38Hz which might be a bit slow. You need to preset TMR0 again in your ISR.

You can change this:

advalue = y
pid_Error = Setpoint - ADValue

To simply read

pid_Error = Setpoint - y

It won't work any better but a little faster and it'll save you a byte or two ;-)

And, try removing the LCDOut statements as a test and see what impact that has on it. Do you have an oscilloscope? Connect it to a pin, set the pin high the first thing you do in the ISR and low again before returning - then you can see if you have the correct interrupt frequency.

[phoenix_1]

ok in UHU or any other PID control if POSITION= SETPOINT motor must clip +/- for to make break in position.
when you set SETPOINT increment +1 or +XXX then PID must move motor in that direction and when MOTOR come to SETPOINT PID must again clip motor +/- in new POSITION becous POSITION is again = SETPOINT.
If you probe to mechanic move MOTOR SHAFT from locked position PID rutine must give reaction to back motor to lock POSITION.
Your PID control work good maybe with L2XX motor driver chip but with real output driver like HP UHU output stage NOT work.
In uhu sistem you have two pins on output stage.One for motor enable / disable (logic 1 enable , logic 0 disable.That is use for motor protect with current detector via resistor in DRAIN of output MOSFETS.
Second pin is in same time PWM drive / direction.
When PWM on these pin is 50/50% that is 512 on 10bit then motor is in stop condition.When on these pin come PWM from 512-0 you have movement of motor from slow 511 to full spid PWM close 10.And when you go from 513 to 1024 you have slow to full speed of motor at close 1014.
My motor have gear 6.25 motor full turn for one shaft full turn.That mean I must use wariable for position storage from 0 - 6250 which represent full range for one full turn of output gear shaft.
When I use your PID rutine it break motor to go from lock position to minus CCW and it give PWM to motor 511 , 510,...492...400 and move motor CW to lock dead point.But when i move output shaft to CW position your PID giwe PWM 511 and normaly that must be to give 513,515....600 to mowe motor in CCW to motor come to lock point.That is becouse in your position is :

'*******************************************************************************
'*******************************************************************************
'Calculate the total drive.

pid_Out = pid_P + pid_I + pid_D         'Calculate total drive....

 pid_Sign = pid_Out.15                   'Save Sign
 pid_Out = ABS pid_Out                   'Convert from two's comp. to abs.
if  pid_Out >= pid_Out_Clamp then        'Output is saturated...
    pid_Status_Out_Sat = 1              'set status bit and...
     pid_out = pid_Out_Clamp '  pid_Out_Clamp = 511 

Endif
If pid_Sign then pid_out = -pid_out     'Re-apply sign.

RETURN                                  'And return to sender.
SkipPID:

Ok and your suggestions are good but I was spend 10..hours and my head is like baloon...sorry.
Regards Robert

[HenrikOlsson]

Hi Robert,
Don't worry, I know the difference between sign & magnitude and locked antiphase.

If you're working with locked anti-phase, like the UHU chip does, then you need to look at the output from the PID filter, if it's positive you add to the dutycycle and if it negative you subtract the ABS value of the PID_Out from the duty cycle. Basically....

'At startup set the PWM dutycycle to 50%
PID_Error = Position - Setpoint
Gosub PID

If PID_Out.15 = 1  then           'Output is negative 
  DutyCycle = 512 - ABS PID_Out
Else                           'Output is positive
  DutyCycle = 512 + PID_Out
Endif

So again, if your dutycycle register is 50% (512) when no torque is delivered (ie. you are working with locked antiphase) then you really should clamp the output of the PID to +/-511, if you don't do that the dutycycle register may over- or underflow. If, for example, the output of the PID filter is allowed to swing to -640, and you subtract that from 512 you end up with -128. -128 in a 16bit variable is the same as 65407 so if you take the lower 10 bits (=895) of that and stuff it into your dutycycle register you'll end up with a dutycycle of 87% instead of 0 - torque will get delivered in wrong direction.....

Also, with a powerstage such as the one on the HP-UHU you need to make sure that dutycycle never reaches 0 or 100% because then the bootstrap capacitors in the highside MOSFET drivers no longer have any time to recharge and bad things will happen.

And, to show you that it does work, here's a link to a video I just uploaded:


Yes, it uses sign & magnitude and no it's not step and direction. It takes serial commands from a host CPU or PC. I've also got a couple of videos there featuring the HP-UHU. I've been quite involved in the HP-UHU project so I know it pretty well.

And as a friendly note, if you think that this PID-code doesn't work for your application then by all means feel free to use something else.

Sincerely,
/Henrik.

[phoenix_1]

No I mean it work good and many thanks to you for writing that...
My best regards and I will continue to test it for my machine.
Regards Robert

[phoenix_1]

I runing your PID rutine in 18F4431 and output is locked antiphase , all work like you explain me . I use two interrupt's one for position reding - QEI module and second to calculate PID , in main loop is only increment of setpoint. No Usart , no DEBUG or LCDOUT.
From time to time not a linear in time it have some little noise which I can also see with scope in PWM out and I can't image from where is ?
Simple motor have jig - agitation in range of couple step's to +/- and after couple next step's it go linear.
I use only CW rotation not CCW all the time and PID becouse it good magnetic and precise break in position.
Can it be from bad tuning of P,I,D parameter's ?
Output amplifier is copy of HP UHU with 4 x IRF540N.
Simple I am lost - it is not disaster but meke me little problems on my machin.
Best regards
\ ROBERT