Professional Documents
Culture Documents
Pendulo Pid
Pendulo Pid
John Charais
Ruan Lourens
Microchip Technology Inc.
INTRODUCTION
The purpose of this application note is to describe how
a PIC16F684 can be used to implement a positional
Proportional-Integral-Derivative (PID) feedback control
in an inherently unstable system. An inverted
pendulum is used to demonstrate this type of control.
The inverted pendulum consists of three main parts:
the base platform, the pendulum and the controller
board, as shown in Figure 1.
FIGURE 1:
INVERTED PENDULUM
DS00964A-page 1
AN964
BASE PLATFORM
FIGURE 2:
MOTOR
PENDULUM
The pendulum is attached to the base platform by a
360 free rotating potentiometer. The pendulums base
is attached to the potentiometer in such a fashion that
when the pendulum is balanced (completely vertical),
the potentiometer center tap is biased to VREF/2. For
the rest of this application note will be used to denote
the displacement angle of the pendulum with respect to
the vertical axis.
CONTROLLER BOARD
FIGURE 3:
DS00964A-page 2
CONTROLLER BOARD
AN964
The controller board has 2 main functions, to measure
and to drive the DC motor. The power supply needed
to run the system is dictated by the selection of the
motor. The motor is controlled by an H-bridge which is
driven by the PIC16F684 Enhanced Capture/Compare/
PWM Module (ECCP). The outputs of the ECCP are
connected to FET drivers that produce the proper drive
voltages and reduce the transition times for the FETs in
the H-bridge.
There are 5 potentiometers located on the controller
board, 3 of which are used for adjusting the PID constants (KP, KI and KD) and one to measure . The fifth
potentiometer is used in conjunction with the input filters reference. The input filter is a low-pass Bessel filter with a cut-off frequency of 60 Hz and has a voltage
gain of 6. A low-pass filter is needed to eliminate any
high frequency noise on the angle measurement which
the derivative term of the PID controller is extremely
sensitive to. The Bessel filter is used because it has the
best response to a step function. (Once the pendulum
is balanced, a sudden displacement that causes it to
become unbalanced will look like a step function.) The
cut-off frequency was chosen to be at least twice the
expected frequency of the pendulum. The gain of the
filter was chosen to increase the resolution of the Analog-to-Digital (A/D) converter. With the 360 potentiometer and a 10-bit A/D converter, with no gain, one LSb
equals 0.35. With the gain set to 6, the displacement
angle is limited to 30 which gives a resolution of
0.059 per LSb. The fifth potentiometer controls the
input filters reference to produce a true 0 displacement angle when the pendulum is vertical. Without this
potentiometer, any slight offset angle will cause the
base to slowly increase its speed and eventually take
the system into an unstable state. For more information
on the controller board, see the schematics in
Appendix A: Schematics.
EQUATION 1:
C(t) = KPE(t) + KI
0 E(t)dt + KDdE(t)/dt
EQUATION 2:
dE(t)/dt ~ [E(n)-E(n-1)]/Ts
PID
FIGURE 4:
EQUATION 3:
0 E(t)dt ~ Ts 0 E(n)
N
R(t)
E(t)
C(t)
Motor
Output
Feedback
Y(t)
EQUATION 4:
C(n) = KPE(n) + KITs E(n) + KD[E(n)- E(n-1)]/Ts
N
OR
N
DS00964A-page 3
AN964
MODELING THE INVERTED
PENDULUM
In order to properly implement the control algorithm,
the user needs to look at how the mechanical and electrical systems are going to interface together. Dynamic
modeling the inverted pendulum is not a simple task.
Here are some of the variables which need to be
looked at in order to model the system:
Bases position
Bases velocity
Bases acceleration
Bases moment of inertia
Bases coefficient of friction
Bases mass
Bases length
Earths gravitational constant
Pendulums position
Pendulums velocity
Pendulums moment of inertia
Pendulums coefficient of friction
Pendulums mass
Pendulums length
EQUATION 5:
= (g/R)
Note:
DS00964A-page 4
AN964
C CODE FLOW CHART
The following flow charts show a simplified version of
the C code for the inverted pendulum. For a full
description of the code, see the AN964 Source Code
file.
FIGURE 5:
FLOW CHART
Main Routine
Reset of
PIC16F684
Reset TMR0
Initialization
Routine
Read Error
Angle
PID Flag
set?
No
Yes
Calculate
Integral Term
Error to
big?
No
Error to
small?
Yes
Clear PID
Loop Flag
No
Return
Calculate
Deriv. Term
Yes
Stop PWM,
Reset terms
Shift errors
clear PID flag
DS00964A-page 5
AN964
SUBTLETIES IN THE SOFTWARE
FIGURE 6:
EQUATION 6:
C CODE
EQUATION 7:
KDdE(t)/dt = TD [E(n)- E(n-1)]/Ts
OR
Where TD = KD/KP
DS00964A-page 6
EQUATION 8:
KDdE(t)/dt = KD [E(n)- E(n-3)]/32
Equation 8 more accurately written would be:
EQUATION 9:
KDdE(t) = TD[E(n)- E(n-3)]/(X3Ts)
=KD[E(n) -E(n-3)]/(KPX3TS)
Where X is an unknown scaling factor
AN964
With a 10-bit A/D, KD is measured as an integer
between 0 and 1023. The X term allows for a fractional
representation of KD, not just a integer. (Similar scaling
factors are also used when calculating the integral and
proportional terms). The 3*TS comes from the fact that
we are tripling our sampling period by using E(n) E(n3) not E(n) E(n-1). Why use E(n) E(n-3) instead of
E(n) E(n-1)? The main reason for doing this is to limit
the variation in the error angle measurement. There will
always be an amount of uncertainty associated with the
error measurement, some of which can be attributed to
A/D error, Bessel filter throughput, mechanical vibration, etc. Since the uncertainty of the error measurements will be the same for all error terms, this
uncertainty can be decreased by effectively tripling the
sampling period. The real key is that the derivative term
is still updated at 256 Hz rate. Doing so gives the benefit of the slower rate a more accurate derivative term,
but at the desired faster sampling rate. The side affect
of doing this is adding approximately 4 milliseconds of
lag to the derivative term, which in this case is acceptable. KP*X*3*TS has been precalculated to be 32 to
increase the speed of the PID loop.
2.
3.
4.
5.
6.
7.
8.
9.
CONCLUSION
By using the PIC16F684 devices ECCP and A/D modules we are able to demonstrate how to implement a
positional PID controller to bring an inherently unstable
system into stability. The keys to implementing this control is to have a basic understanding of the mechanical
system, and identifying the derivative term would be a
critical factor in the overall stability of the system. The
other keys, with respect to the software, were making
sure our registers never overflow, and picking the
frequency of the PID loop that is a power of 2 so we
could have a fast multiply and divide routine using the
left and right shift.
DS00964A-page 7
DS00964A-page 8
Note 1:
2:
3:
R2
100 k
P1B
R1
100 k
P1A
7
4
2
5
U1:B
TC4428A
3 U1:A
TC4428A
8 7
Q1:B
+15V
Q1:A
IRF7389
IRF7389
6 5
+15V
D2
BAT54S
C3
10 pF
C2
10 pF
+15V
C6
C7
100 pF
100 nF
J1
1 2
MOTOR
PIC16F684/P
VSS
RA0/AN0
RA1/AN1
RA2/AN2
RC0/AN4
RC1/AN5
RC2/AN6
C4
10 pF
1K
R5
+15V
Q2:A
7 8
IRF7389
D3
BAT54S
5 6
+15V
TC4428A
U2:B
5
U2:A
TC4428A 3
C8
100 nF
SENSOR
Q2:B 3 IRF7389
ICSPDATA
+15V
C5
10 pF
+15V
14
13
12
ICSPCLK
11
P-POT
10
I-POT
9
D-POT
8
P1D
Unless otherwise specified, resistance values are in . Resistors are 5% tolerance. Capacitance values are in F.
Device names and numbers shown here are for reference only and may differ from the actual number.
Items labeled with A are unpopulated, B are socketed but not populated, and C are socketed and populated.
C1
100 nF
MCLR
P1A
P1B
P1C
BUTTON
VDD
RA5/OSC1
RA4/OSC2
RA3/MCLR
RC5/ECCP
RC4
RC3/AN7
U3
R4
100 k
P1D
R3
100 k
P1C
APPENDIX A:
+15V
C9
100 nF
+5_SWITCHED
1
2
3
4
5
6
7
AN964
SCHEMATICS
FIGURE 7:
R28
10 k
R26
10 k
R27
10 k
CCW
CW
+5V
CCW
CW
+5V
CCW
CW
+5V
R31
100
0.1 F
C22
R30
100
0.1 F
C23
R29
1
2
3
100
0.1 F
C24
Angle
Pot.
J2
+5V
3
D-POT
I-POT
P-POT
0.1 F
C10
C11
0.1 F
MCP602SN
8
4
TP2
TP1 +15V
C18
1 F
470 F
1 F
C14
U8
22 F
C17
MCP602SN
U4:B
7
6.8 nF
C13
Output 3
LM2940S
75 k
R15
9.1 k
R16
1 Input
1 F
C12
C19
R18
150 k
CCW
CW
+5V
R17
150 k
1.5 k
R14
R9
Offset
10 k
U4:A
1
3
1
+5V
Gnd
2
Gnd
TP4
R23
R8
TP3
4.7 F
C21
BFMP
J3
4.7 F
C20
+5V
R33
10 k
TP5 TP6
+5_SWITCHED
4
2
S1
1
1
2
3
4
5
6
MCLR
+5_SWITCHED
ICSPCLK
ICSPDATA
No Connect
In-Circuit Serial
Programming Header
BUTTON
+5_SWITCHED
SENSOR
MCLR
R32
10 k
+5_SWITCHED
AN964
FIGURE 8:
DS00964A-page 9
AN964
APPENDIX B:
DERIVATION OF
EQUATION 5
FIGURE 9:
EQUATION B-1:
Position = Rsini +Rcosj
Where R is the length of the pendulum
The pendulums angular velocity is the derivative of the
position with respect to .
EQUATION B-2:
i
mg
EQUATION B-3:
Acceleration
EQUATION B-4:
Force = mR(cos i sinj 2sini 2cosj)
With a free body diagram we can see that there are 2
forces acting on the pendulum, the tension from the rod
and the force of gravity. This free body diagram yields
the following equation.
EQUATION B-5:
Force = Tsin i + Tcos j mgj
DS00964A-page 10
AN964
Equate Equation B-4 and Equation B-5:
EQUATION B-6:
Tsin i + Tcosj mgj = m R(cos i sin j 2sini 2cosj)
Now separate into 2 different vectors equations and
eliminate the vector notation.
EQUATION B-7:
(i Vector)
(j Vector)
EQUATION B-9:
Tsincos = m Rcos2 mR2sincos
Multiply Equation B-8 by sin.
EQUATION B-10:
Tsincos mgsin = -m Rsin2 mR2sincos
Moving -mgsin to the other side.
EQUATION B-11:
Tsincos = mgsin mRsin2 mR2sincos
Equate Equation B-9 and Equation B-11.
EQUATION B-12:
m Rcos2 mR2sincos = mgsin mRsin2 mR2sincos
Divide by each side by mR.
EQUATION B-13:
DS00964A-page 11
AN964
EQUATION B-14:
EQUATION B-15:
= (g/R)sin
Using the small angle approximation where sin =
yields.
EQUATION B-16:
= (g/R)
DS00964A-page 12
Note the following details of the code protection feature on Microchip devices:
Microchip products meet the specification contained in their particular Microchip Data Sheet.
Microchip believes that its family of products is one of the most secure families of its kind on the market today, when used in the
intended manner and under normal conditions.
There are dishonest and possibly illegal methods used to breach the code protection feature. All of these methods, to our
knowledge, require using the Microchip products in a manner outside the operating specifications contained in Microchips Data
Sheets. Most likely, the person doing so is engaged in theft of intellectual property.
Microchip is willing to work with the customer who is concerned about the integrity of their code.
Neither Microchip nor any other semiconductor manufacturer can guarantee the security of their code. Code protection does not
mean that we are guaranteeing the product as unbreakable.
Code protection is constantly evolving. We at Microchip are committed to continuously improving the code protection features of our
products. Attempts to break Microchips code protection feature may be a violation of the Digital Millennium Copyright Act. If such acts
allow unauthorized access to your software or other copyrighted work, you may have a right to sue for relief under that Act.
Trademarks
The Microchip name and logo, the Microchip logo, Accuron,
dsPIC, KEELOQ, microID, MPLAB, PIC, PICmicro, PICSTART,
PRO MATE, PowerSmart, rfPIC, and SmartShunt are
registered trademarks of Microchip Technology Incorporated
in the U.S.A. and other countries.
AmpLab, FilterLab, Migratable Memory, MXDEV, MXLAB,
PICMASTER, SEEVAL, SmartSensor and The Embedded
Control Solutions Company are registered trademarks of
Microchip Technology Incorporated in the U.S.A.
Analog-for-the-Digital Age, Application Maestro, dsPICDEM,
dsPICDEM.net, dsPICworks, ECAN, ECONOMONITOR,
FanSense, FlexROM, fuzzyLAB, In-Circuit Serial
Programming, ICSP, ICEPIC, MPASM, MPLIB, MPLINK,
MPSIM, PICkit, PICDEM, PICDEM.net, PICLAB, PICtail,
PowerCal, PowerInfo, PowerMate, PowerTool, rfLAB,
rfPICDEM, Select Mode, Smart Serial, SmartTel and Total
Endurance are trademarks of Microchip Technology
Incorporated in the U.S.A. and other countries.
SQTP is a service mark of Microchip Technology Incorporated
in the U.S.A.
All other trademarks mentioned herein are property of their
respective companies.
2004, Microchip Technology Incorporated, Printed in the
U.S.A., All Rights Reserved.
Printed on recycled paper.
DS00964A-page 13
ASIA/PACIFIC
ASIA/PACIFIC
EUROPE
Corporate Office
2355 West Chandler Blvd.
Chandler, AZ 85224-6199
Tel: 480-792-7200
Fax: 480-792-7277
Technical Support:
http://support.microchip.com
Web Address:
www.microchip.com
Australia - Sydney
Tel: 61-2-9868-6733
Fax: 61-2-9868-6755
India - Bangalore
Tel: 91-80-2229-0061
Fax: 91-80-2229-0062
China - Beijing
Tel: 86-10-8528-2100
Fax: 86-10-8528-2104
Austria - Weis
Tel: 43-7242-2244-399
Fax: 43-7242-2244-393
Denmark - Ballerup
Tel: 45-4450-2828
Fax: 45-4485-2829
China - Chengdu
Tel: 86-28-8676-6200
Fax: 86-28-8676-6599
Japan - Kanagawa
Tel: 81-45-471- 6166
Fax: 81-45-471-6122
France - Massy
Tel: 33-1-69-53-63-20
Fax: 33-1-69-30-90-79
China - Fuzhou
Tel: 86-591-8750-3506
Fax: 86-591-8750-3521
Korea - Seoul
Tel: 82-2-554-7200
Fax: 82-2-558-5932 or
82-2-558-5934
Germany - Ismaning
Tel: 49-89-627-144-0
Fax: 49-89-627-144-44
Atlanta
Alpharetta, GA
Tel: 770-640-0034
Fax: 770-640-0307
Boston
Westford, MA
Tel: 978-692-3848
Fax: 978-692-3821
Chicago
Itasca, IL
Tel: 630-285-0071
Fax: 630-285-0075
Dallas
Addison, TX
Tel: 972-818-7423
Fax: 972-818-2924
Detroit
Farmington Hills, MI
Tel: 248-538-2250
Fax: 248-538-2260
Kokomo
Kokomo, IN
Tel: 765-864-8360
Fax: 765-864-8387
Singapore
Tel: 65-6334-8870
Fax: 65-6334-8850
Taiwan - Kaohsiung
Tel: 886-7-536-4818
Fax: 886-7-536-4803
Taiwan - Taipei
Tel: 886-2-2500-6610
Fax: 886-2-2508-0102
Italy - Milan
Tel: 39-0331-742611
Fax: 39-0331-466781
Netherlands - Drunen
Tel: 31-416-690399
Fax: 31-416-690340
England - Berkshire
Tel: 44-118-921-5869
Fax: 44-118-921-5820
Taiwan - Hsinchu
Tel: 886-3-572-9526
Fax: 886-3-572-6459
China - Qingdao
Tel: 86-532-502-7355
Fax: 86-532-502-7205
Los Angeles
Mission Viejo, CA
Tel: 949-462-9523
Fax: 949-462-9608
San Jose
Mountain View, CA
Tel: 650-215-1444
Fax: 650-961-0286
Toronto
Mississauga, Ontario,
Canada
Tel: 905-673-0699
Fax: 905-673-6509
10/20/04
DS00964A-page 14