PID SPEED CONTROL WITH ARDUINO

1. Required Hardware
- Arduino Uno (01).
- DC motor with pre-assembled encoder (Faulhaber 2342L012CR) (01).
- Resistor 10K (03), 100 (01), 50 (01).
- Button (01).
- Green LED (01).
- Power supply > 12VDC (01).
- Breadboard and connecting wires.
2. Hardware Connection Diagram

Fig. 1 Hardware Connection Diagram
3. PID Block Diagram
The following diagram shows connections between function blocks to establish a closed loop to control
the motor speed.
HHManh, Mechatronics, AIT Thailand. Page 1



Fig. 2 Block Diagram of the Experiment
The encoder attached with the motor shaft at the back of the motor will follow the rotation of the
motor. The generated pulse trains by the encoder will become inputs for the micro-controller to
calculate the motor shaft speed in RPM. The speed in RPM later is converted into percentage by
software and become feedback to the PID controller.
The speed setpoint in RPM sent by the user to micro-controller will be converted to percentage and
become the setpoint for the PID controller.
Based on the setpoint and feedback (both are in %) the controller will calculate its output in percentage.
Then PWM converter will generate a pulse train that its pulse width dependent on the PID output value.
PWM later is sent to the motor drive to move the motor.
4. PID Algorithm
We will use the ideal, dependent PID form with derivative on error for our experiment since this form is
used most popular. From the PID background, we have the form as the following:
( )
. ( ) ( ) .
bias
Kc de t
CO CO Kc e t e t dt Kc Td
Ti dt
= + + +

Where,
CO = controller output signal.
CO
bias
= controller bias, set by bumpless transfer.
e(t) = current controller error, defined as SP PV.
SP = set point.
PV = measured process variable.
Kc = controller gain (also called proportional gain), a tuning parameter.
Ti = reset time, a tuning parameter (or sometimes called Pre-Act).
Td = derivative time, a tuning parameter.

PID
Algorithm
PWM
Converter
MOTOR
DRIVER
MOTOR
ENCODER
% Conversion
Speed
Calculation
% Conversion
MicroController
Speed Setpoint (RPM)
(settingSpeed)
RPM Setpoint (%)
(setPoint)
Motor Speed (%)
(processVariable)
Motor Speed (RPM)
(rotationSpeed)
PID output (%)
(lmn)
Motor Shaft
operationMode
(AUTO =1, MANUAL =0)
manualValue
controlVariable
HHManh, Mechatronics, AIT Thailand. Page 2

Using nomenclature is up to the controller vendor since there is a little standardization on it. I prefer to
use the nomenclature a little bit different from the above formula as followings. The bias value doesnt
appear in the formula since this value will be the value set by user at manual mode:
lmn = +
1

lmn = lmnP + lmnI + lmnD
lmnP = GAIN * ER
lmnI = GAIN
1

lmnD = GAIN

Where,
lmn = loop manipulated variable or controller output signal (unit is %).
lmnP = proportional portion.
lmnI = integral portion.
lmnD = derivative portion.
ER = current controller error, defined as setPoint processVariable (unit is %).
GAIN = controller gain (also called proportional gain), a tuning parameter (no unit).
TI = reset time, a tuning parameter (or sometimes called Pre-Act) (unit is ms).
TD = derivative time, a tuning parameter (unit is ms).
setPoint = set point (unit is %).
processVariable = measured process variable (unit is %).

The PID forms written above are in continuous form and used by analog controller. Using
microcontroller, forms should be converted into digital form. In microcontroller, the calculation will be
done at fixed interval called CYCLE.
4.1 PID Portions
- Proportional Portion
lmnP = GAIN * ER
lmnP = GAIN * (setPoint processVariable)

- Integral Portion
lmnI = GAIN *
1

d(lmnI)/dt = GAIN * (1/TI) * ER
d(lmnI) = GAIN * (1/TI) * ER * dt
lmnI lastLmnI

= GAIN * ER * (1/TI) * dt
lmnI = lastLmnI + lmnP * (1/TI) * dt
lmnI = lastLmnI + lmnP * (1/TI) * CYCLE
lmnI = lastLmnI + (CYCLE/TI) * lmnP

- Derivative Portion
lmnD = GAIN

= (TD/dt) * GAIN * d(ER)

lmnD = (TD/dt) * GAIN * (ER lastER)
lmnD = (TD/dt) * (GAIN * ER GAIN * lastER)
lmnD = (TD/CYCLE) * (lmnP lastLmnP)
HHManh, Mechatronics, AIT Thailand. Page 3


Where,
lastLmnI : Integral portion at the last interval.
lastLmnP : Proportional at the last interval.

4.2 Anti Integral Windup and Bumpless Mode Transfer
To get the convenience when programming, all constants and PID parameters are given names in capital
letters. The PID block is written as a function and this function will be called to be processed at one
interval (CYCLE). There will be global and local variables. Global variables could be accessed at any time
by any program section where area local variables could be accessed only when the related function is
processed.
In addition to PID parameters and CYCLE, there will be 04 important variables of the controller:
- rotationSpeed : this is speed in RPM as the feedback and input to the controller.
- settingSpeed : this is speed setpoint in RPM by the user and is input to the controller.
- controlVariable : this is the controller output in percentage.
- manualValue : this is the value set by the user when system in manual mode.

const bool ean AUTO = t r ue; / / aut omat i c ( PI D) mode.
const bool ean MANUAL = f al se; / / manual mode ( speed cont r ol by manual i nput val ue.
const doubl e HI GH_LI MI T = 100. 0; / / hi gh l i mi t .
const doubl e LOW_LI MI T = 0. 0; / / l ow l i mi t .
/ *var i abl es used f or PI D*/
doubl e r ot at i onSpeed = 0. 0; / / r ot at i on speed i n RPM.
doubl e set t i ngSpeed = 0; / / set t i ng speed.
doubl e cont r ol Var i abl e; / / cont r ol var i abl e.
doubl e GAI N = 0. 015; / / pr opor t i onal gai n.
doubl e TI = 2000; / / i nt er gr al t i me i n ms.
doubl e TD = 0; / / der i vat i ve t i me i n ms.
doubl e CYCLE = 100; / / PI D pr ocessi ng i nt er val .
doubl e manual Val ue = 0; / / manual val ue.


Variables used locally at PID section are called local variables. They are used as intermediate places for
storing and to remember the value calculated at last interval.

doubl e pr ocessVar i abl e; / / speed i n per cent age.
doubl e set Poi nt ; / / set speed i n per cent age.
doubl e l mn; / / l oop mani pul at ed var i abl e
doubl e l mnP; / / pr opor t i onal por t i on.
doubl e l mnD; / / der i vat i ve por t i on.
doubl e i nt egr al Di f f ; / / I nt egr al di f f er ence
st at i c doubl e l mnI ; / / i nt egr al por t i on. St at i c var s f or r emember i ng l ast val ue.
st at i c doubl e l ast LmnP; / / l ast val ue of l mnP.

Since PID controllers work with i/o in percentage, then firstly we need to convert inputs in engineering
unit (RPM) in percentage (it is therefore the working range needs to know).


pr ocessVar i abl e = map( r ot at i onSpeed, 0, 130, 0, 100) ;
set Poi nt = map( set t i ngSpeed, 0, 130, 0, 100) ;

The proportional portion is then calculated:

/ / Pr opor t i onal por t i on cal cul at i on
l mnP = GAI N * ( set Poi nt - pr ocessVar i abl e) ;

HHManh, Mechatronics, AIT Thailand. Page 4

At auto mode, there are two important cases: 1) the integral is positive and the controller output is over
the high limit. 2)the integral is negative and the controller output is less than the low limit. To get the
safety, it they happen then the integral part should not either increase more or decrease more.

/ / at aut o mode
i f ( oper at i onMode == AUTO) {
/ / I nt egr al por t i on cal cul at i on.
i f ( ( ( i nt egr al Di f f > 0) && ( l mn > HI GH_LI MI T) ) | | ( ( i nt egr al Di f f < 0) && ( l mn <
LOW_LI MI T) ) ) {i nt egr al Di f f = 0. 0; }
el se { i nt egr al Di f f = l mnP * CYCLE/ TI ; }
l mnI += i nt egr al Di f f ;


/ / Der i vat i ve por t i on cal cul at i on.
l mnD = ( TD/ CYCLE) * ( l mnP - l ast LmnP) ;
l ast LmnP = l mnP; / / updat e l mnP
l mn = l mnP + l mnI + l mnD; / / sumof por t i ons

For safety, when the output and the integral is also over the limit. The output is set to the limit and the
integral portion is adjusted.

/ / Saf et y l i mi t
i f ( ( l mn > HI GH_LI MI T) && ( l mnI > HI GH_LI MI T) ) {
l mnI = l mnI - ( l mn - HI GH_LI MI T) ; }
i f ( ( l mn < LOW_LI MI T) && ( l mnI < LOW_LI MI T) ) {
l mnI = l mnI + ( LOW_LI MI T - l mn) ; }
i f ( l mn > HI GH_LI MI T) {l mn = HI GH_LI MI T; }
i f ( l mn < LOW_LI MI T) {l mn = LOW_LI MI T; }

To get a bumpless transfer from the auto to manual mode, then at auto mode, the manual value should
follow the controller output.
manual Val ue = l mn; }
To get a bumpless transfer from the manual to auto mode, then at manual mode, the controller output
is set equal to the manual value and the integral value is back calculated. The speed setpoint is set equal
to the feedback speed.

/ / At manual mode
el se {l mn = manual Val ue; l mnI = l mn - l mnP; set t i ngSpeed = r ot at i onSpeed; }
cont r ol Var i abl e = l mn; }

4.3 Program Code

/ *===================================================================================================*/
/ *PROGRAM I NTRODUCTI ON
Mot or speed cont r ol by PI D cont r ol l er . Bot h channel A and B of t he encoder ar e connect ed t o
i nt er r upt 0( pi n2) and i nt er r upt 1 ( pi n3) t o cal cul at e speed. The f or mul a i s as bel l owi ng:
r ot at i onSpeed=60000*posi t i onDi f f er ence/ ( Pul sePer Revol ut i on*DECODE_NUMBER*t i meDi f f er ence*GEAR_RATI O) .
Const ant s and par amet er s ar e denot ed by capi t al l et t er s. E. g. : HI GH_LI MI T ( =100. 0) .
HHManh, Mechat r oni cs, AI T Thai l and. */
/ *===================================================================================================*/
/ *CONSTANTS AND GLOBAL VARI ABLE DECLARATI ON*/
const i nt ENCODER_PI NA = 2; / / pi n f or encoder ' s channel A.
const i nt ENCODER_PI NB = 3; / / pi n f or encoder ' s channel B.
const i nt DI R1 = 6; / / di r ect i on 1, DI R1=0, DI R=1, CWr ot at i on.
const i nt DI R2 = 7; / / di r ect i on 2, DI R1=0, DI R=1, CWr ot at i on.
const i nt BUTTON_PI N = 8; / / mode: FALSE = AUTO, TRUE = MAN.
const i nt LED_PI N = 9; / / LED connect i on.
const i nt ENA = 11; / / PWM out put pi n connect i ng t o t he dr i ver .
const i nt PPR = 12; / / number of pul ses per r evol ut i on by encoder .
const i nt GEAR_RATI O = 64; / / gear r at i on f r ommot or t o mai n shaf t .
const i nt DECODE_NUMBER = 4; / / depends on usi ng i nt er r upt ( see encoder sect i on) .
const bool ean AUTO = t r ue; / / aut omat i c ( PI D) mode.
const bool ean MANUAL = f al se; / / manual mode ( speed cont r ol by manual i nput val ue.
const doubl e HI GH_LI MI T = 100. 0; / / hi gh l i mi t .
const doubl e LOW_LI MI T = 0. 0; / / l ow l i mi t .
const i nt DEBOUNCE_TI ME = 50; / / debounce t i me i n ms.
/ *var i abl es used f or PI D*/
doubl e r ot at i onSpeed = 0. 0; / / r ot at i on speed i n RPM.
HHManh, Mechatronics, AIT Thailand. Page 5

doubl e set t i ngSpeed = 0; / / set t i ng speed at manual mode.
doubl e cont r ol Var i abl e; / / cont r ol var i abl e.
doubl e GAI N = 0. 015; / / pr opor t i onal gai n.
doubl e TI = 2000; / / i nt er gr al t i me i n ms.
doubl e TD = 0; / / der i vat i ve t i me i n ms.
doubl e CYCLE = 100; / / PI D pr ocessi ng i nt er val .
doubl e manual Val ue = 0; / / manual val ue.
bool ean oper at i onMode = f al se; / / t el l i ng t he oper at i on mode. Manual = t r ue, Aut o = f al se.
bool ean l ast Oper at i onMode = f al se;
/ / var s i n I nt er r upt Subr out i ne need t o be vol at i l e.
vol at i l e l ong cur r ent Posi t i on = 0; / / number of pul ses at t i me of cal cul at e
i nt dut yCycl eMap = 0; / / dut y cycl e val ue af t er mappi ng 0 - 255.
l ong l ast Posi t i on = 0; / / l ast number of pul ses used i n speed cal cul at e.
l ong posi t i onDi f f er ence = 0; / / di f f er ence i n pul se number ( used i n speed cal cul at e) .
unsi gned l ong t i meDi f f er ence = 0; / / di f f er ence i n t i me used i n speed cal cul at e.
unsi gned l ong l ast Di spl ayTi me; / / i nt er val t i me var i n ms f or di spl ay.
unsi gned l ong l ast SpeedTi me; / / i nt er val t i me var i n ms f or RPM cal cul at e.
/ *Fol l owi ng var s f or pr event i ng but t on bounce f r ompr ess. */
unsi gned l ong l ast DebounceTi me;
i nt but t onSt at e, l ast But t onSt at e, bi t Toggl e;
/ *===================================================================================================*/
voi d set up( ) {
TCCR2B = TCCR2B & 0b11111000 | 0x01; / / PWM f r equency changed t o 31250Hz
pi nMode( BUTTON_PI N, I NPUT) ; / / set pi n BUTTON_PI N as i nput .
pi nMode( ENCODER_PI NA, I NPUT) ; / / set pi n ENCODER_PI NA as i nput .
pi nMode( ENCODER_PI NB, I NPUT) ; / / set pi n ENCODER_PI NB as i nput .
pi nMode( LED_PI N, OUTPUT) ; / / set pi n LED_PI N as out put .
pi nMode( DI R1, OUTPUT) ; / / set pi n DI R1 as out put .
pi nMode( DI R2, OUTPUT) ; / / set pi n DI R2 as out put .
pi nMode( ENA, OUTPUT) ; / / set pi nt ENA as out put
/ / di gi t al Wr i t e( DI R1, LOW) ; / / by wr i t t i ng pi n DI R1 t o LOWand pi n DI R2 t o HI GH. CWdi r ect i on i s set .
/ / di gi t al Wr i t e( DI R2, HI GH) ;
at t achI nt er r upt ( 0, doEncoder A, CHANGE) ; / / set up I SR f or i nt er r upt 0 - pi n 2.
at t achI nt er r upt ( 1, doEncoder B, CHANGE) ; / / set up I SR f or i nt er r upt 1 - pi n 3.
Ser i al . begi n( 9600) ; / / ser i al speed 9600kb/ s.
/ / Ser i al . pr i nt l n( " OPERATI NG I NFORMATI ON" ) ;
Ser i al . pr i nt l n( " Q + ENTER f or PI D par amet er di spl ay or ot her s f or par amet er change! " ) ;
Ser i al . pr i nt l n( " P + ENTER => GAI N = GAI N + 0. 001, p + ENTER => GAI N = GAI N - 0. 001" ) ;
Ser i al . pr i nt l n( " I + ENTER => TI = TI + 10 ( ms) , i + ENTER => TI = TI - 10 ( ms) " ) ;
Ser i al . pr i nt l n( " D + ENTER => TD = TD + 10 ( ms) , d + ENTER => TD = TD - 10 ( ms) " ) ;
Ser i al . pr i nt l n( " AUTOMATI C mode: Pr ess ' set t i ng- speed' + C => speed set poi nt " ) ;
Ser i al . pr i nt l n( " MANUAL mode: Set di r ect i on and dut y- cycl e ( 0 - 100) t o r ot at e t he mot or : " ) ;
Ser i al . pr i nt l n( " ( R+ENTER => CWdi r ect i on, L+ENTER => CCWdi r ect i on, ' dut y- cycl e' +C+ENTER => Speed" ) ;
Ser i al . pr i nt ( " S+ENTER => Sl ow st op, B+ENTER => Fast st op) " ) ; Ser i al . pr i nt l n( " G+ENTER => i nf or mat i on" ) ;
Ser i al . pr i nt l n( " Ready i n MANUAL mode! " ) ; }
/ *===================================================================================================*/
voi d l oop( ) {
/ *Thi s par t i s f or pr event i ng bounce f r ompr ess t he mode but t on. The t oggl e but t on i s used t o
change t he mode. LED ON or OFF by pr essi ng t he but t on. LED ON = AUTOMATI C mode, LED OFF = MANUAL MODE. */
i nt r eadi ngBut t on = di gi t al Read( BUTTON_PI N) ;
i f ( r eadi ngBut t on ! = l ast But t onSt at e) {l ast DebounceTi me = mi l l i s( ) ; }
i f ( ( mi l l i s( ) - l ast DebounceTi me) > DEBOUNCE_TI ME) {
i f ( r eadi ngBut t on ! = but t onSt at e) {
but t onSt at e = r eadi ngBut t on;
i f ( but t onSt at e == HI GH) { bi t Toggl e = ! bi t Toggl e; }}}
l ast But t onSt at e = r eadi ngBut t on;
di gi t al Wr i t e( LED_PI N, bi t Toggl e) ; / / Wr i t i ng t o LED.
oper at i onMode = di gi t al Read( LED_PI N) ; / / oper at i on mode i s r esul t ed f r omLED st at us.
/ *- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
Di spl ay set t i ng speed, cont r ol var i al e, di r ect i on */
i f ( mi l l i s( ) - l ast Di spl ayTi me > 2000) {
i f ( cur r ent Posi t i on ! = l ast Posi t i on) {
i f ( oper at i onMode == MANUAL) {
Ser i al . pr i nt ( " Set t i ng manual _val ue = " ) ; Ser i al . pr i nt ( manual Val ue) ; Ser i al . pr i nt ( " %" ) ;
Ser i al . pr i nt ( " . Cont r ol _var i abl e = " ) ; Ser i al . pr i nt ( cont r ol Var i abl e) ; Ser i al . pr i nt l n( " %" ) ;
Ser i al . pr i nt ( " Real speed = " ) ; Ser i al . pr i nt ( r ot at i onSpeed) ; Ser i al . pr i nt ( " RPM" ) ;
Ser i al . pr i nt ( " . Fol l owed set t i ng_speed = " ) ; Ser i al . pr i nt ( set t i ngSpeed ) ; Ser i al . pr i nt l n( " RPM" ) ;
Ser i al . pr i nt l n( " - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - " ) ; }
i f ( oper at i onMode == AUTO) {
Ser i al . pr i nt ( " Set t i ng_speed = " ) ; Ser i al . pr i nt ( set t i ngSpeed ) ; Ser i al . pr i nt ( " RPM" ) ;
Ser i al . pr i nt ( " . Real speed = " ) ; Ser i al . pr i nt ( r ot at i onSpeed) ; Ser i al . pr i nt l n( " RPM" ) ;
Ser i al . pr i nt ( " Cont r ol _var i abl e = " ) ; Ser i al . pr i nt ( cont r ol Var i abl e) ; Ser i al . pr i nt ( " %" ) ;
Ser i al . pr i nt ( " . Fol l owed manual _val ue = " ) ; Ser i al . pr i nt ( manual Val ue) ; Ser i al . pr i nt l n( " %" ) ;
Ser i al . pr i nt l n( " - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - " ) ; }
l ast Di spl ayTi me = mi l l i s( ) ; }}
/ *- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
RPM speed cal cul at i on f or mul a:
r ot at i onSpeed = 60000. 0*posi t i onDi f f er ence/ ( PPR*DECODE_NUMBER*GEAR_RATI O*t i meDi f f er ence)
PPR = 12, decodeNumnber = 4, GEARr at i o = 64, 60000/ ( 12*4*64) = 19. 53125. */

i f ( mi l l i s( ) - l ast SpeedTi me > 50) {
HHManh, Mechatronics, AIT Thailand. Page 6

t i meDi f f er ence = mi l l i s( ) - l ast SpeedTi me;
posi t i onDi f f er ence = cur r ent Posi t i on - l ast Posi t i on;
i f ( posi t i onDi f f er ence < 0) {posi t i onDi f f er ence = - posi t i onDi f f er ence; }
r ot at i onSpeed = 19. 53125 * posi t i onDi f f er ence/ t i meDi f f er ence;
l ast SpeedTi me = mi l l i s( ) ; / / updat e speed cal cul at i on t i me.
l ast Posi t i on = cur r ent Posi t i on; }/ / updat e number of count s f or posi t i on.
/ *- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
Par amet er s and oper at i on mode di spl ay */
i f ( ( l ast Oper at i onMode == MANUAL) && ( oper at i onMode == AUTO) ) {
Ser i al . pr i nt l n( " Syst emi s changed t o AUTOMATI C mode! " ) ;
Ser i al . pr i nt l n( " Pr ess ' set t i ng- speed' +C+ENTER => speed set poi nt ! " ) ; }
/ / Ser i al . pr i nt l n( " ==================================================================" ) ; }
i f ( ( l ast Oper at i onMode == AUTO) && ( oper at i onMode == MANUAL) ) {
Ser i al . pr i nt l n( " Syst emi s changed t o MANUAL mode! " ) ;
Ser i al . pr i nt l n( " ( S+ENTER => Sl ow st op, B+ENTER => Fast st op) " ) ; }
/ / Ser i al . pr i nt l n( " ==================================================================" ) ; }
l ast Oper at i onMode = oper at i onMode;
/ *- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
I nt er f ace f or manual mode */
i f ( Ser i al . avai l abl e( ) ) {
byt e ch = Ser i al . r ead( ) ;
i f ( oper at i onMode == MANUAL) {
st at i c doubl e dut yCycl e;
swi t ch( ch) {
case ' R' : di gi t al Wr i t e( DI R1, LOW) ; di gi t al Wr i t e( DI R2, HI GH) ;
Ser i al . pr i nt l n( " CWr ot at i on di r ect i on i s set ! " ) ;
/ / Ser i al . pr i nt l n( " =============================" ) ;
Ser i al . pr i nt l n( " Set dut y- cycl e ( 0 - 100) : ' dut y- cyl e' +C+ENTER " ) ; br eak;
case ' L' : di gi t al Wr i t e( DI R1, HI GH) ; di gi t al Wr i t e( DI R2, LOW) ;
Ser i al . pr i nt l n( " CCWr ot at i on di r ect i on i s set ! " ) ;
/ / Ser i al . pr i nt l n( " =============================" ) ;
Ser i al . pr i nt l n( " Set dut y- cycl e ( 0 - 100) : ' dut y- cyl e' +C+ENTER " ) ; br eak;
case ' 0' . . . ' 9' :
dut yCycl e = dut yCycl e * 10 + ch - ' 0' ; br eak;
case ' C' :
i f ( ( dut yCycl e >= 0) && ( dut yCycl e <= 100) ) {manual Val ue = dut yCycl e;
Ser i al . pr i nt ( " Dut y- cycl e i s set at : " ) ; Ser i al . pr i nt ( dut yCycl e) ; Ser i al . pr i nt l n( " %! " ) ;
Ser i al . pr i nt ( " dut yCycl eMap = " ) ; Ser i al . pr i nt ( dut yCycl e) ; Ser i al . pr i nt l n( " / 255" ) ;
Ser i al . pr i nt l n( " ==============================" ) ; }
el se {Ser i al . pr i nt l n ( " Wr ong set t i ng, ent er val ue f r om0 - 100! " ) ;
Ser i al . pr i nt l n( " xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx" ) ; }
dut yCycl e = 0; br eak;
case ' S' : di gi t al Wr i t e( DI R1, LOW) ; di gi t al Wr i t e( DI R2, LOW) ; manual Val ue = 0;
Ser i al . pr i nt l n( " Sl ow st op i s act i vat ed! " ) ; br eak;
case ' B' : di gi t al Wr i t e( DI R1, HI GH) ; di gi t al Wr i t e( DI R2, HI GH) ; manual Val ue = 0;
Ser i al . pr i nt l n( " Fast st op i s act i vat ed! " ) ; br eak; }}
/ *- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
I nt er f ace f or aut omat i c mode */
el se {
st at i c doubl e sendSet Poi nt = 0;
swi t ch( ch) {
case ' 0' . . . ' 9' : sendSet Poi nt = sendSet Poi nt * 10 + ch - ' 0' ; br eak;
case ' C' :
i f ( ( sendSet Poi nt >= 0) && ( sendSet Poi nt <= 130) ) {set t i ngSpeed = sendSet Poi nt ;
Ser i al . pr i nt ( " Set poi nt speed i s : " ) ; Ser i al . pr i nt ( set t i ngSpeed) ; Ser i al . pr i nt l n( " RPM" ) ; }
el se {Ser i al . pr i nt l n ( " Wr ong set t i ng, ent er val ue f r om0 - 130 onl y! " ) ;
Ser i al . pr i nt l n( " xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx" ) ; }
sendSet Poi nt = 0; br eak; }}
/ *Thi s bel ow sect i on i s f or PI D par amet er modi f i cat i on and di spl ay. At any t i me, pr essi ng Q wi l l
di spl ay PI D par amet er s. Capi t al l et t er s f or i ncr ease and l ower case ones f or decr ease of par amet er s.
Af t er any change, 03 par amet er s ar e shown. */
swi t ch( ch) {
case ' Q' :
Ser i al . pr i nt ( " GAI N = " ) ; Ser i al . pr i nt l n( GAI N, 4) ; / / pr i nt 04 di gi t s af t er deci mal poi nt .
Ser i al . pr i nt ( " TI = " ) ; Ser i al . pr i nt ( TI ) ; Ser i al . pr i nt l n( " mi l l i seconds" ) ;
Ser i al . pr i nt ( " TD = " ) ; Ser i al . pr i nt ( TD) ; Ser i al . pr i nt l n( " mi l l i seconds" ) ;
Ser i al . pr i nt l n( " ========================================================" ) ; br eak;
case ' P' : GAI N += 0. 001; / / GAI N = GAI N + . . .
Ser i al . pr i nt ( " GAI N i s changed t o: " ) ; Ser i al . pr i nt l n( GAI N, 4) ; / / pr i nt 04 di gi t s af t er deci mal poi nt .
Ser i al . pr i nt l n( " ========================================================" ) ; br eak;
case ' p' : GAI N - = 0. 001;
Ser i al . pr i nt ( " GAI N i s changed t o: " ) ; Ser i al . pr i nt l n( GAI N, 4) ; / / pr i nt 04 di gi t s af t er deci mal poi nt .
Ser i al . pr i nt l n( " ========================================================" ) ; br eak;
case ' I ' : TI += 10;
Ser i al . pr i nt ( " TI i s changed t o: " ) ; Ser i al . pr i nt ( TI ) ; Ser i al . pr i nt l n( " mi l l i seconds" ) ;
Ser i al . pr i nt l n( " ========================================================" ) ; br eak;
case ' i ' : TI - = 10;
Ser i al . pr i nt ( " TI i s changed t o: " ) ; Ser i al . pr i nt ( TI ) ; Ser i al . pr i nt l n( " mi l l i seconds" ) ;
Ser i al . pr i nt l n( " ========================================================" ) ; br eak;
case ' D' : TD += 10;
Ser i al . pr i nt ( " TD i s changed t o: " ) ; Ser i al . pr i nt ( TD) ; Ser i al . pr i nt l n( " mi l l i seconds" ) ;
HHManh, Mechatronics, AIT Thailand. Page 7

Ser i al . pr i nt l n( " ========================================================" ) ; br eak;
case ' d' : TD - = 10;
Ser i al . pr i nt ( " TD i s changed t o: " ) ; Ser i al . pr i nt ( TD) ; Ser i al . pr i nt l n( " mi l l i seconds" ) ;
Ser i al . pr i nt l n( " ========================================================" ) ; br eak; }}
/ / case ' G' :
/ / Ser i al . pr i nt l n( " OPERATI NG I NFORMATI ON" ) ;
/ / Ser i al . pr i nt l n( " Pr ess Q + ENTER f or PI D par amet er di spl ay or ot her s f or par amet er change! " ) ;
/ / Ser i al . pr i nt l n( " P + ENTER => GAI N = GAI N + 0. 001, p + ENTER => GAI N = GAI N - 0. 001" ) ;
/ / Ser i al . pr i nt l n( " I + ENTER => TI = TI + 10 ( ms) , i + ENTER => TI = TI - 10 ( ms) " ) ;
/ / Ser i al . pr i nt l n( " D + ENTER => TD = TD + 10 ( ms) , d + ENTER => TD = TD - 10 ( ms) " ) ;
/ / Ser i al . pr i nt l n( " AUTOMATI C mode: Pr ess ' set t i ng- speed' + C => speed set poi nt " ) ;
/ / Ser i al . pr i nt l n( " MANUAL mode: Set di r ect i on and dut y- cycl e ( 0 - 100) t o r ot at e t he mot or : " ) ;
/ / Ser i al . pr i nt l n( " ( R + ENTER => CWdi r ect i on, L + ENTER => CCWdi r ect i on, ' dut y- cycl e' + C => Speed" ) ;
/ / Ser i al . pr i nt l n( " S + ENTER => Sl ow st op, B + ENTER => Fast st op) " ) ; br eak; }}
/ *- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
PI D f unct i on i s pr ocess at i nt er val of CYCLE*/
unsi gned l ong l ast PI DTi me; / / t o st or e t he r eal - t i me when pr evi ous execut i on i s done.
i f ( mi l l i s( ) - l ast PI DTi me > CYCLE) {
PI D( ) ;
l ast PI DTi me = mi l l i s( ) ; }
/ *- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
Bel ow f or mappi ng t o di gi t al val ue ( 0- 255) and wr i t e t o PWM out put . */
dut yCycl eMap = map( cont r ol Var i abl e, 0, 100, 0, 255) ; / / Mappi ng mani pul at ed var t o di gi t al val ue.
anal ogWr i t e( ENA, dut yCycl eMap) ; }/ / Wr i t e pul se t r ai n t o PWM out put pi n
/ / END OF LOOP
/ *===================================================================================================*/
/ *PI D ALGORI THM
l mn i s out put of t he PI D cont r ol l er and consi st s of 03 por t i ons: pr opor t i onal , i nt egr al , & der i vat i ve.
l mn ( cal l ed l oop- mani pul at ed var i abl e) : l mn = l mnP + l mnI + l mnD.
l mn = GAI N*( ER + ( 1/ TI ) *i nt egr al ( ER*dt ) + TD*d( ER) / dt ) ( dt = CYCLE: pr ocessi ng i nt er val ) .
l mnP = GAI N*ER = GAI N * ( set Poi nt - pr ocessVar i abl e) . set Poi nt & pr ocessVar i abl e ( i n per cent 0 - 100) .
l mnI = GAI N*( 1/ TI ) *i nt egr al ( ER*dt ) <=> d( l mnI ) = GAI N*( 1/ TI ) *ER*dt = l mnP*( 1/ TI ) * CYCLE
<=> l mnI - l ast LmnI = l mnP*( 1/ TI ) * CYCLE <=> l mnI = l ast LmnI + l mnP*CYCLE/ TI .
l mnD = GAI N*TD*d( ER) / dt = TD* d( GAI N*ER) / dt = TD*d( l mnP) / dt = TD * ( l mnP - l ast LmnP) / CYCLE.
l mnD = ( TD/ CYCLE) *( l mnP - l ast LmnP)
Bumpl ess manual - t o- aut o swi t ch: i n manual mode, l mn = manual Val ue, set t i ngSpeed = r ot at i onSpeed
t hen back- cal cul at e l mnI .
Bumpl ess aut o- t o- manual swi t ch: i n aut o mode manual Val ue = Cont r ol Var i abl e.
Pr event i ng i nt egr al - wi ndup: when l mn i s out of l i mi t we set l mn t o l i mi t and back cal cul at e l mnI . */
voi d PI D( )
{
doubl e pr ocessVar i abl e; / / speed i n per cent age.
doubl e set Poi nt ; / / set speed i n per cent age.
doubl e l mn; / / l oop mani pul at ed var i abl e
doubl e l mnP; / / pr opor t i onal por t i on.
doubl e l mnD; / / der i vat i ve por t i on.
doubl e i nt egr al Di f f ; / / I nt egr al di f f er ence
st at i c doubl e l mnI ; / / i nt egr al por t i on. St at i c var s f or r emember i ng l ast val ue.
st at i c doubl e l ast LmnP; / / l ast val ue of l mnP.
pr ocessVar i abl e = map( r ot at i onSpeed, 0, 130, 0, 100) ;
set Poi nt = map( set t i ngSpeed, 0, 130, 0, 100) ;
/ / Pr opor t i onal por t i on cal cul at i on
l mnP = GAI N * ( set Poi nt - pr ocessVar i abl e) ;
/ / at aut o mode
i f ( oper at i onMode == AUTO) {
/ / I nt egr al por t i on cal cul at i on.
i f ( ( ( i nt egr al Di f f > 0) && ( l mn > HI GH_LI MI T) ) | | ( ( i nt egr al Di f f < 0) && ( l mn < LOW_LI MI T) ) ) {
i nt egr al Di f f = 0. 0; }
el se { i nt egr al Di f f = l mnP * CYCLE/ TI ; }
l mnI += i nt egr al Di f f ;
/ / Der i vat i ve por t i on cal cul at i on.
l mnD = ( TD/ CYCLE) * ( l mnP - l ast LmnP) ;
l ast LmnP = l mnP; / / updat e l mnP
l mn = l mnP + l mnI + l mnD; / / sumof por t i ons
/ / Saf et y l i mi t
i f ( ( l mn > HI GH_LI MI T) && ( l mnI > HI GH_LI MI T) ) {
l mnI = l mnI - ( l mn - HI GH_LI MI T) ; }
i f ( ( l mn < LOW_LI MI T) && ( l mnI < LOW_LI MI T) ) {
l mnI = l mnI + ( LOW_LI MI T - l mn) ; }
i f ( l mn > HI GH_LI MI T) {l mn = HI GH_LI MI T; }
i f ( l mn < LOW_LI MI T) {l mn = LOW_LI MI T; }
manual Val ue = l mn; }
/ / At manual mode
el se {l mn = manual Val ue; l mnI = l mn - l mnP; set t i ngSpeed = r ot at i onSpeed; }
cont r ol Var i abl e = l mn; }

/ *===================================================================================================*/
/ *I NTERRUPT SUBROUTI NE CHANNEL A
When an i nt er r upt on channel A occur s t hen: CWr ot at i on: A ! = B, CCWr ot at i on: A = B. 8*/
voi d doEncoder A( ) {

HHManh, Mechatronics, AIT Thailand. Page 8

i f ( di gi t al Read( ENCODER_PI NA) ! = di gi t al Read( ENCODER_PI NB) ) {
cur r ent Posi t i on++; }
el se {cur r ent Posi t i on- - ; }}
/ *===================================================================================================*/
/ *I NTERRUPT SUBROUTI NE CHANNEL B
When an i nt er r upt on channel B occur s t hen: CWr ot at i on: A = B, CCWr ot at i on: A ! = B. */
voi d doEncoder B( ) {
i f ( di gi t al Read( ENCODER_PI NA) == di gi t al Read( ENCODER_PI NB) ) {
cur r ent Posi t i on++; }
el se {cur r ent Posi t i on- - ; }}
/ *END OF PROGRAM
====================================================================================================*/

REFERENCES
APPENDICES
HHManh, Mechatronics, AIT Thailand. Page 9