Design, Modeling, and

Fabrication of Twin Rotor Control

System
v
ABSTRACT
This thesis presents the Design, Modeling, and Fabrication of Twin
Rotor Control System The twin rotor system has high non!linearity and
signi"!cant cross co#pling between its two a$es This pro%ect aims
designing and developing a control scheme for the twin rotor control
system
The mechanical model of twin rotor system consists of two rotors, a main
rotor and a tail rotor, mo#nted at each end on a lever bar The primary tas&
is to control the ang#lar position of the twin rotor system 't has two degrees
of freedom which are the a(im#thal angle and the elevation angle As a "rst
step, mathematical modeling of the twin rotor system is done As the system
is highly non!linear, so nonlinear e)#ations of a(im#th and elevation angles
are lineari(ed Then a State Space obesrver based controller is designed for
the system By implementing the controller in Sim#lin& and testing it on the
physical model it is fo#nd that controller is able to stabili(e the system and
trac& a reference signal
A m#ltif#nction embedded control card is #sed for real time motion con!
trol of the two rotors *ere a microcontroller is connected with two Digital
to Analog Converter+DAC, and Analog to Digital Converter+ADC, channels
thro#gh dierent latches An e$ternal -./ memory is #sed for the addressing
schemes of the DAC and ADC0s The codes for chec&ing the f#nctionality
of the DAC0s,ADC and observer design are written in C lang#age #sing /eil
software
vi
Chapter 1
'2TR3D4CT'32
The Twin Rotor Control System demonstrates the principle of a non!linear
m#lti!inp#t m#lti!o#tp#t +M'M3, system Systems having more than one
control inp#t and more than one o#tp#t are m#lti!inp#t!m#lti!o#tp#t sys!
tems The twin rotor system is a nonlinear M'M3 system with signi"cant
cross co#pling Change in one inp#t variable sim#ltaneo#sly aects all o#t!
p#t variables in case of a M'M3 system
The twin rotor system resembles a helicopter model 5enerally helicopter
systems have a single main rotor, b#t the tor)#e t#rns the rotor against its
air drag ca#sing the body of the helicopter to t#rn in the direction opposite
to that of rotor To balance this eect, some anti!tor)#e is re)#ired The tail
rotor design is introd#ced to balance the tor)#e eect by p#shing or p#lling
against the tail rotor
The state of the twin rotor control system is described by fo#r process vari!
ables6 the hori(ontal and vertical angle of rotation of the two rotors and the
two inp#t voltages The re)#irement of the pro%ect is to control the ang#!
lar position of the vertical and hori(ontal a$is of twin rotor system 't has
strong co#pling between the two a$is The model needs to be lineari(ed "rst
and then controllers m#st be designed to control the positions of lever bar
The system consists of two dc motors which are #sed to drive the two rotors
The embedded motion control board is serially interfaced with a comp#ter
so that #ser sitting in front of comp#ter can provide commands and can see
c#rrent position of the two rotors
1
Fig#re
116
Repre
sentat
ion of
M'M
3
syste
m
11 Analysis and Control of M'M3 System
A control problem cannot be f#rther red#ced if the system has more than
one control inp#t and more than one manip#lated variable The interaction
between them is that the model cannot be f#rther red#ced The re)#ire!
ment in case of a M'M3 system is that system sho#ld allow for independent
manip#lation of its o#tp#t variable Any change of an inp#t variable sim#l!
taneo#sly aects all o#tp#t variables 't is said that there is a strong cross
co#pling between dynamic channels of the process This co#pling creates
vario#s di"c#lties to the process So, a controller design for a M'M3 sys!
tem m#st incl#de an attempt to eliminate the cross co#pling between its
inp#t!o#tp#t channels Mathematically the transfer f#nction obtained for
the system which relates the o#tp#t vector and reference signal is a diagonal
matri$
1. *ardware Description
The system consists of vertical a$is on which a lever arm is connected At
the two ends of the lever arm, two rotors are attached which are rotated at
78 degrees from each other allowing to generate hori(ontal and vertical
thr#sts Two potentiometers are attached to snese the position
1 At the %#nction between the vertical a$is and the lever arm, a poten!
tiometer is connected to feedbac& the position in the vertical plane
. Another potentiometer is connected on the vertical a$is which is #sed
to feedbac& the position in the hori(ontal plane
.
Fig#re 1.6 Rotor System
" The rotor generating vertical thr#st is called the main rotor This
enables the model to pitch which prod#ces elevation angle, which is
rotation in the vertical plane aro#nd the hori(ontal 9 a$is
" The rotor generating the hori(ontal thr#st is called the tail rotor This
enables the model to yaw to prod#ce a(im#thal angle, which is rotation
in the hori(ontal plane aro#nd the vertical : a$is
Apart from mechanical #nit, twin rotor system also has an electrical #nit 't
plays an important role in control of twin rotor system The electrical #nit is
interconnected with a comp#ter to send and receive control signals which are
then send to the two motors to drive the twin rotor system The mechanical
and electrical #nits provide a complete control system set!#p
1- ;ro%ect 3b%ectives
" 4nderstanding the mathematical model of twin rotor system
" ;rogramming the e)#ations in MAT<AB and Sim#lin&
" <ineari(ation of the system
" Stabili(ing the system thro#gh controller design
-
" Design of the hardware and control cards
" Fabrication, assembling and testing of the system
1= Thesis <ayo#t
The thesis contains following chapters6
Chapter 1 is abo#t introd#ction of M'M3 systems and its importance in
control engineering *ardware description of twin rotor system is also given
here This chapter also contains pro%ect ob%ectives and re)#irements
Chapter . is abo#t mathematical modeling of twin rotor system which e$!
plains its &inematics and energy e$pressions Chapter also incl#des e)#ations
of motion that are derived and implemented in Sim#lin& with the given pa!
rameters
Chapter - is abo#t lineari(ation of the system 't describe how to create
a S!f#nction "le and its implementation in Sim#lin& <ineari(ed system is
obtained by e$ec#ting MAT<AB commands
Chapter = is abo#t the controller design in state space This chapter dis!
c#ss pole placement approach and design of a servo mechanism #sing pole
placement 't also incl#des design of a f#ll order state observer
Chapter > is abo#t the design of electrical #nit of twin rotor system 't
describes the hardware and software con"g#rations of the real time motion
control embedded card
Chapter ? is abo#t concl#sion of the thesis 't disc#sses f#t#re recommen!
dations, targets and scope of the pro%ect
=
Chapter .
MAT*@MAT'CA<
M3D@<'25
'n this chapter, basic #nderstanding of the mathematical modeling of twin
rotor control system is developed The dynamics of the system is derived
by #sing the @#ler <agrangian approach The system has two degrees of
freedom which are
1 Rotation " in hori(ontal plane, abo#t the vertical a$is
. Rotation " in the vertical plane, abo#t the hori(ontal a$is
<agrangian approach is mathematically e)#al to 2ewton0s approach of
apply forces to things and see how they move 'nstead of force here
we deal with &inetic and potential energies of the system The core of
<agrangian approach is a )#antity called <agrangian +<,
.1 /inematics
Ass#ming that the mass distrib#tion on the lever arm of twin rotor is re!
stricted to a line between the two rotors, which is at distance h from the
pivot point The origin on this line is denoted by 80 <et Ar
$
+R,,r
y
+R,,r
(
+R,B
denotes an arbitrary point ; on the lever arm which is at distance R from
30 Then
r
$
+R, C Rcos " cos " hsin " cos " +.1,
r
y
+R, C Rcos " sin " hsin " sin " +..,
>
Fig#re .16
Representation of
system in Cartesian
coordinate
r
(
+R
, C
Rsi
n "
D
hcos
"
+.-,
By dierentiating
e)#ation .1, .. and .- with repeat to time corresponding velocities are
obtainedA1B
.. @nergy @$pressions
The <agrangian approach deals with &inetic and potential energies of the
system Ee &now that &inetic energy T of a body depends on velocity v
of the ob%ect and the potential energy F depends on the position ( of the
ob%ect as shown in e)#ation .= and .>
1
/6@ C T C
.
mv
.
+.=,
;6@ C F C mg( +.>,
The parameters position ,mass and velocity are a f#nction of the distance R
So the derived e)#ations are
1
G
T C
.
v
.
+R,dm,R, +.?,
?
C "

C "
G
F C g (+R,+R,dm+R, +.H,
*ere g is the acceleration d#e to gravity
.- @)#ations of Motion
The <agrangian states that6
< C T F +.I,
<agrange0s contains partial derivatives of the <agrangian with respect to the
position and velocity of the ob%ect So the e)#ations of motion are6
d
"
J<
"
J<
"
dt
J"
J"
"
d
"
J<
"
J<
"
dt
J"
J"
"
+.7,
+.18,
Consider l is the total length of the lever arm,m is the mass of the lever
arm,l
c
is the center of gravity of the lever arm, K
A
is the inertia of the center
a$le and K
<
is the inertia of the lever arm The two e)#ations of motion are
solved for " and "
d
"
J"
J<

dt
"J<"

J"
C

K
<
D mh
.
"
" A +cos " sin ", K
<
"
D ml
c
hcos
.
" ml
c
hsin
.
" D h
.
sin " cos "B"
.
D mgl
c
cos " D mghsin " C "
"
d
"
J"
J<
" " "
" "

dt
"J<"

J"
C A .K
<
sin " cos "" D .ml
c
hcos
.
"" .ml
c
hsin
.
"" D .mh
.
sin " cos """B D AK
<
cos
.
" D
.ml
c
hsin " cos " D mh
.
sin
.
" D K
A
B" C "
"
These e)#ations can be e$pressed in the matri$ form as6
" "
" "
"

" "
"
"
"
D+"L ",
"
D C+"L "L "L ",
"
D g+"L ", C " +.11,
H
*ere the term with coe"cient D is representing inertia matri$, the term with
C represents centripetal force matri$ and g represents gravity matri$ Ehere
d d
"
"
D C
"d
11
d
1.
"
.1 ..
C C
c
11
c
1.
8
"
c
.1
c
..
"
g C
mgl
c
cos " D mg sin "
.= @)#ations of Rotors
The motor dynamics for the two rotors can be e$pressed as6
d 1
1
R
1
6
dt
M
1
C
T
1
M
1
D
&
1
T
1
#
1
+.1.,
d
1 1
R
.
6
dt
M
.
C
T
.
M
.
D
&
.
T
.
#
.
+.1-,
*ere T
1
, T
.
are the time constants of the motors, &
1
and &
.
are the motor
constants of the motors and M
1
, M
.
are the ang#lar velocities of the main
rotor and the tail rotor respectively
.> Sim#lation Model
The sim#lation model of the twin rotor system can be described by consid!
ering the e)#ations of rotors and e)#ations of motion
"
d 1
" "
" "
"
dt
" C
D
Ad
..
"
"
+ .K
<
sin " cos " D .mhl
c
+cos
.
" sin
.
", D .mh
.
sin " cos ",""B
d
1.
"
"
AK
<
sin " cos " mhl
c
+cos
.
" sin
.
",B"
.
+mgl
c
cos " D mghsin ",
d 1
" " "
" "
"
dt
" C
D
Ad
.1
"
"
+ .K
<
sin " cos " D .mhl
c
+cos
.
" sin
.
", D .mh
.
sin " cos ",""B
d
11
"
"
AK
<
sin " cos " mhl
c
+cos
.
" sin
.
",B"
.
+mgl
c
cos " D mghsin ",
The total mass m is
m C m
1
D m
.
D m
l
D m
w
+.1=,
I
*ere m
1
is the mass of main rotor, m
.
is the mass of tail rotor and m
w
is
the mass of motors and "ns m
l
is the mass of lever arm bar The s#m of
the moment of inertia for the point masses of the rotors and weight and solid
lever bar is the moment of inertia of the lever arm
. . .
l
m D
- -
1 .
K
<
C m
1
l
1
D m
.
l
.
D m
w
l
w
D
-l
l
1
D l
l
.
+.1>,
The center of mass for the system is6
.
m
l
c
C
m
1
l
1
m
.
l
.
m
w
l
w
D
m
l
+l
1
l
.
,
+.1?,
The verticle a$le has a negligible moment of inertia K
A
C 8
.? ;arameters
l1C1H=L NArm length to main rotor
l.C1H>1L NArm length to tail rotor
mlC88H=-.L NMass of main rotor+finsDmotorD+baseDfasteners,
mwC18.?L NMass of weight
lwC8>1IL NDistance from pivot to weight
m1C=1=HL NMass of lever arm bar
M1C-H?HL NMass of tail rotor+finsDmotorD+baseDfasteners,
gC7IL Nacc d#e to gravity
T1C>L NTime constant for main rotor
T.C.>L NTime constant for tail rotor
&1C88>>L NMotor constant for main rotor
&.C88==L NMotor constant for tail rotor
&"phiC888.L NCoefficient of friction of vertical a$le bearing
&"theetaC888.L NCoefficient of friction of hori(ontal a$le bearing
MvC=?.7.O18P!>L NAerodynamic lift coefficient for Main Rotor
MhC.H777O18P!>L NAerodynamic drag coefficient for Main Rotor
TvC1.>H.O18P!>L NAerodynamic drag coefficient for Tail Rotor
7
ThCH8H7-O18P!>L NAerodynamic lift coefficient for Tail Rotor
mCmlDm1Dm.DmwL Ntotal mass
KAC8L Nmoment of inertia for vertical a$le
K<C+m1O+l1Ol1,,D+m.O+l.Ol.,,D+mwO+lwOlw,,D++mlQ-,O+++l1Ol1Ol1,D
+l.Ol.Ol.,,Q+l1Dl.,,,L Nmoment of inertia for hori(ontal a$le
lcC++mlO+l1!l.,Q.,D+m1Ol1,!+m.Ol.,!+mwOlw,,QmL N center of gravity
d..CK<D+mOh.Oh.,L
18
Chapter -
<'2@AR'GAT'32 3F T*@
S:ST@M
-1 2onlinear Systems
For a nonlinear system the principle of s#perposition is not applicable 'n
case of a nonlinear two inp#t system the response cannot be treated by
considering single inp#t at a time and adding the res#lts 'n general there
are very few systems in control that are act#ally linear
-11 <ineari(ation of 2onlinear Systems
'n control engineering the response of the system can be treated aro#nd an
e)#ilibri#m point A nonlinear system may be treated as a linear system if
its operation is considered within some limits or aro#nd e)#ilibri#m points
The twin rotor system has two degrees of freedom6 the elevation and a(!
im#thal angles The cross co#pling between these two angles is the reason for
the non!linearity in twin rotor system The mathematical model developed
for the twin rotor control system is also a non!linear model <ineari(ation of
this non!linear system is re)#ired in order to implement control f#nction 'n
this pro%ect Sf#nction approach is #sed to obtain a linear model
11
-. S!F#nction
S!f#nctions +system!f#nctions, provide a powerf#l mechanism for e$tending
the capabilities of the Sim#lin& environment An S!f#nction is a comp#ter
lang#age description of a Sim#lin& bloc& written in MAT<AB, C, CDD,
or Fortran C, CDD, and Fortran S!f#nctions are compiled as M@9 "les
#sing the me$ #tility As with other M@9 "les, S!f#nctions are dynamically
lin&ed s#bro#tines that the MAT<AB interpreter can a#tomatically load and
e$ec#te
S!f#nctions #se a special calling synta$ that enables yo# to interact with the
Sim#lin& engine S!f#nctions follow a general form and can accommodate
contin#o#s and discrete systems By following a set of simple r#les, yo# can
implement an algorithm in an S!f#nction and #se the S!F#nction bloc& to
add it to a Sim#lin& model After yo# write yo#r S!f#nction and place its
name in an S!F#nction bloc& +available in the 4ser!De"ned F#nctions
bloc& library,, yo# can c#stomi(e the #ser interface #sing S!F#nction bloc&
to add it to a Sim#lin& model
-.1 Erite an S!f#nction "le
As the nonlinear model becomes more comple$, the dierential e)#ations
associated with it can be written in an M!"le These M!"les will be called by
Sim#lin& thro#gh the S!f#nction bloc&
'np#t Arg#ments
" t ! the time variable
" $ ! the state variable vector
" # ! the inp#t vector +'t will be called in Sim#lin& bloc&,
"
ag ! an integer val#e that indicates which tas& is to be performed at a
partic#lar time There are si$ types of
ag re)#ests that Sim#lin& performs, with
ag val#es 8,1,.,-,= and 7
3#tp#t Arg#ments
" sys ! This is the vector that shows the res#lts of the re)#est sent by
Sim#lin&
1.
Flag val#e
8
1
.
-
=
7
Table -16 S!F#nction
ags Description
De"nes S!F#nction bloc& characteristics, sample time, initial
conditions and si(es
Calc#lates the derivatives of the contin#o#s state variables
4pdates discrete states, sample times, and ma%or time step
Re)#irements
Calc#lates the o#tp#ts of the S!f#nction
Calc#lates the time of the ne$t hit in absol#te time for discrete
domain
;erforms any necessary end of sim#lation tas&s
All other o#tp#t arg#ments will be eval#ated by Sim#lin& only when
ag C 8, otherwise they are ignored6
" $8 ! initial condition vector
" str ! This is set to a n#ll matri$
" ts ! This represents the sampling time interval 'n case of contin#o#s
time systems it is set to (ero
The description of the S!f#nction M!"le developed for this pro%ect is given in
Appendi$ A
-.. 4sing S!f#nction bloc&s in Sim#lin&
'n the Sim#lin& <ibrary browser, go to the 4ser!De"ne F#nctions s#bdirec!
tory Then drag!drop the level. MAT<AB S!F#nction bloc& Do#ble
clic& the S!f#nction bloc& and change the name as 0sf#nction0 Also "ll
in the parameters *ere 0sf#nction0 is the name of the M!"le created
An advantage of #sing S!f#nctions is that yo# can b#ild a general!p#rpose
bloc& that yo# can be #sed in dierent problems An M!"le of an S!F#nction
bloc& m#st provide information abo#t the model characteristics Sim#lin&
needs this information d#ring sim#lation As the sim#lation precedes,
Sim#lin&, the 3D@ solver, and the M!"le interact to perform speci"c tas&s
These tas&s
1-
Fig#re -16 S!
f#nction bloc& in
Sim#lin&
incl#de de"ning initial
conditions and bloc&
characteristics, and
comp#ting derivatives,
discrete states, and the system o#tp#ts
-- <ineari(ation 4sing MAT<AB
After the development of S!f#nction bloc& in Sim#lin&,the system is lineari(ed
#sing linmod command in MAT<AB linmod obtains linear models from sys!
tems of ordinary di e)#ations +3D@s, AA,B,C,DBClinmod+Rsf#nbloc&0,$8,#8,
obtains the state!space linear model of the system of ordinary dierential
e)#ations described in the bloc& diagram Ssf#nbloc&T *ere Ssf#nbloc&T is
the name of o#r S!f#nction "le
The state vector $, and inp#t # can be speci"ed by #sing trim command in
MAT<AB A linear model will then be obtained at this operating point The
M!"le created for these commands is shown in Appendi$ B These "les are
e$ec#ted to obtain state!space linear system
The operating points and the state!space matrices are shown here6
$8 C
? H
? H
? H
.
168888
-
?
868888
H
?
86>>>7
H
?
86888
H
=
>868888
>
--6?=18
#8 C
"
86>888
"
86=?H?
1=
?
?
?
8
H
H
H
?
?
?
8
H
H
H
1
?
?
?
8
H
H
H
.
8
?
8
?
A C
?8
=
8
8
168888 8 8
8611>. 868888 8
8 8 168888
8 1H611H8 868HH=
8 8 8
8 8 8
8 8
-
868.H= 868=8I
H
8 8
H
868->H 8688>H
H
867871 8
>
8 -68-8-
.
8
?
8
?
B C
? 8
=
7867871
8
-
8
H
8
H
8
H
8
>
8 .1I688H=
"
C C
8
8 8 8 8 8
"
8 1 8 8 8
.
8 ?
8
?
D C
?8
=
8
8
8
-
8
H
8
H
8
H
8
>
8
1>
Fig#re -.6 ;lot of <ineari(ed System
The plots for the si$ state variables speci"ed in the S!f#nction M!"le are
obtained after lineari(ation of the system
1?
Chapter =
C32TR3< S:ST@M D@S'52
'2 STAT@ S;AC@
=1 'ntrod#ction
This chapter disc#sses State!space design method for designing and
analy(ing feedbac& control system for the twin rotor system as the plots
obtained d#ring lineari(ation of the system shows that system is highly
#nstable State!space controller design is the modern approach and preferred
over fre)#ency domain methods of design beca#se fre)#ency domain
analysis do not allow #s to specify all closed loop poles in systems of order
higher than . as they do not allow #s to get information abo#t s#"cient
n#mber of #n&nown parameters to place all the closed loop poles of the
system #ni)#ely State!space method solve this problem of pole placement
as6
1 't introd#ces other ad%#stable parameters
. 't de"nes vario#s techni)#es for "nding #n&nown parameter val#es to
place the closed loop poles of the system
Firstly, pole placement approach for reg#lator system is applied and state
feedbac& gain matri$ is obtained #sing MAT<AB commands Then design
of a servo system #sing the pole placement approach is disc#ssed A f#ll
order observer is implemented and transfer f#nctions of observer controllers
for a(im#thal and elevation angles are derived Finally, )#adratic optimal
control method is disc#ssed
1H
Fig#
re
=16
Stat
e
spac
e
repr
esen
tation of a ;lant
=. ;ole ;lacement
Ee ass#me that all state variables are meas#rable and are available for feed!
bac& 'f the system response is completely state controllable, then poles of
the closed!loop system can be placed at any desired locations thro#gh state
feed!bac& by introd#cing an appropriate state feedbac& gain matri$ Ee
decided that the desired closed!loop poles are to be at KC A!1D1iOs)rt+-, !1!
1iOs)rt+-, !1 !. !- !=B, where K is the matri$ consisting of the locations of
the desired closed loop poles By choosing an appropriate state feedbac&
gain matri$ for the system, it is possible to force the system to have closed!
loop poles at the desired locations, provided that the original system is
completely state controllable
Consider the plant is represented in state space as6
$" C A$ D B# +=1,
y C C$ +=.,
Ehere A C ?$? constant matri$ B C ?$. constant matri$ C C .$? constant
matri$ $ C state vector +?$1, : C o#tp#t signal +.$1, # C control signal
+.$1, The bloc& diagram representation of these e)#ations is 'f all the state
variables are feedbac& to the control, thro#gh a gain /, then there will be
n gains, /
n
that co#ld be ad%#sted to yield the re)#ired closed!loop pole
val#es The feedbac& thro#gh the gain / is shown in Fig#re =. by the
feedbac& vector !/ The state e)#ations for the closed!loop system now will
be
$" C A$ D B# C A$ D B+ /$, C +A B/,$ +=-,
y C C$ +==,
This implies that the control signal # can be determined by an instantaneo#s
1I
Fig#re =.6 ;lant with state variable feedbac&
state S#ch a scheme is called state feedbac& This closed!loop system has
no inp#t 'ts ob%ective is to maintain the (ero o#tp#t As the dist#rbances
are present in any system, the o#tp#t will deviate from (ero The non(ero
o#tp#t will ret#rn to the (ero reference signal beca#se of the state feedbac&
controller of the system The controllers can be divided into . categories
3ne is the reg#latory systems in which the reference signal is a constant
)#antity +incl#ding (ero, The other control scheme is the one in which
reference is a time varying )#antity 'n o#r pro%ect we implemented the
reg#lator system
=.1 Solving ;ole ;lacement ;roblem Eith MAT<AB
;ole!placement problems can be solved easily with MAT<AB
MAT<AB has two commands ac&er and place for the comp#tation of
feedbac& gain matri$ / / C place+A,B,K, comp#tes a state!feedbac&
matri$ / s#ch that the eigenval#es of A!B/ are those speci"ed in the
vector K @igen val#es m#st not have a m#ltiplicity factor greater the inp#t
vector For single inp#t system, the commands ac&er and place gives the
same / 'n case of m#lti!inp#t m#lti!o#tp#t systems, the command place
sho#ld be #sed instead of ac&er
To #se the command ac&er or place, we sho#ld have &nown the matrices A,
B and K
A matri$ B matri$, K matri$
where KC A!1D1iOs)rt+-, !1!1iOs)rt+-, !1 !. !- !=B
For o#r system as the matri$ A is ?$?, so we choose length of the matri$ K
to be 1$? Feedbac& gain matri$ / is calc#lated #sing place command
17
/ C
Fig#
re
=-6
Resp
onse
of
@lev
atio
n
and
A(i
m#t
hal
angl
e to
step
inp#
t
"
86.I81
861H-8
86==8> >H611=?
86.=H. .67>.7
1-6--I.
86H8II
868I1I 86881?
"
8688>= 86881=
=.. Choosing the <ocations of Desired Closed!<oop
;oles
There is a necessary condition for placing the closed loop poles at desired
locations that the system m#st be state controllable The stability of this
system can be determined by eval#ating the eigenval#es of matri$ A!B/The
approached #sed here is to choose poles #sing fre)#ency domain analysis,
by placing a dominant pair of closed!loop poles and then choose the other
poles s#ch that they are far to the left of the dominant closed!loop poles 'f
the dominant closed!loop poles are placed far from the %M a$is, the system
response will become very fast, and the signals in the system will become
very large, which res#lts that the system may become nonlinearThis
approach m#st be avoided eig +A!B/, C
.8
!18888 D 1H-.1i
!18888 ! 1H-.1i
!=8888
!18888
!-8888
!.8888
The eigen val#es of matri$ A!B/ shows that the closed loop poles are stable
=- State 3bservers
Controller design is based #pon meas#rable state variables for feedbac&
thro#gh ad%#stable gain matrices *owever in practical, some of the state
variables are not available to feedbac&, send them to the controller or it is
too costly to meas#re them 'f the state variables are not meas#rable directly
beca#se of system limitations or cost, it is possible to estimate the states
@stimated states are then fed to the controller An observer, sometimes
called an es!timator, is #sed to calc#late state variables that are not
meas#rable from the plant directly 'n this pro%ect the derivatives of
elevation and a(im#thal angles are not available to meas#re so an observer
is designed to estimate their val#es *ere the observer is a model of the
plant
The plant with an observed state will have
P
$" C A$P D B# D /e+y CyP, +=>,
yP C C$P +=?,
Ehere hat of $ is the estimated state and C hat of $ is the estimated o#tp#t
The inp#ts to the observer are the o#tp#t y and the control inp#t # Matri$
/e which is called the observer gain matri$, is a weighting matri$ to the
correction term involving the dierence between the meas#red o#tp#t y and
the estimated o#tp#t The design of the observer is separate from the design
of the controller Similar to the design of the controller vector, /, the design
of the observer consists of eval#ating the constant vector, /e, so that the
transient response of the observer is faster than the response of the controlled
loop in order to yield a rapidly #pdated estimate of the state vector
.1
/e C
?
?
?
H
H
H
Fig#re ==6 Bloc&
diagram of observered
state feedbac& control
=-1 3bserver 5ain
Matri$ /e, with
MAT<AB
Beca#se of the
d#ality of pole!
placement and
observer design,
the same algo!
rithm can be
applied to both
the pole!placement
problem and the observer!design problem Th#s, the commands ac&er and
place can be #sed to de!termine the observer gain matri$ /e /e C
place+A,B,<, comp#tes a state!feedbac& matri$ /e s#ch that the
eigenval#es of A!B/e are those speci"ed in the vector < As a general
r#le, the closed loop poles in < m#st be . to "ve times of the closed loop
poles chosen for matri$ K
.
868.88
?
861817
?
8688-H
?
868.II
=
-67=-?
86?1>>
8688=I
-
868>?=
H
868.1H
H
861-?8
H
16I>>-
>
16>1-7
== Design of reg#lator systems with observers
Following proced#re is followed for designing reg#lator system based con!
troller6
..
Fig#re =>6 Response of
Angle ;hi and Theta to
'nitial conditions
1 Derive a state!space
model of the plant
. Choose the
desired
closed!loop
poles for
pole
placement
Choose the
desired
observer
poles
- Determine the state feedbac& gain matri$ / and the observer gain
matri$ /e
= 4sing the gain matrices / and /e obtained in step -, derive the
transfer f#nction of the observer controller 'f it is a stable controller,
chec& the response to the given initial condition 'f the response is not
acceptable, ad%#st the closed!loop pole location andQor observer pole
location #ntil an acceptable response is obtained
Ee shall now consider the optimal reg#lator problem that, given the system
e)#ation 'n MAT<AB the command l)r+A,B,C,D, solves the contin#o#s
time linear, )#adratic reg#lator problem and the associate Ricatti e)#ation
This command calc#lates the feedbac& gain matri$ / After calc#lating the
feedbac& gain matri$ /, response of the system to the initial condition was
accomplished
.-
Chapter >
D@S'52 3F @<@CTR'CA<
42'T
>1 'ntrod#ction
The electrical #nit of twin rotor system plays an important role in position
control of the two rotors of twin rotor system The electrical #nit is intercon!
nected with a comp#ter 'ts p#rpose is to send and receive control signals
which are then send to the two motors to drive the twin rotor system ;#lse
Eidth Mod#lation techni)#e is generally #sed to control speed and direction
of dc motors 3ne method is to generate analog or variable voltage from a
microcontroller, by #sing a digital to analog converter +DAC, 't prod#ces
an o#tp#t voltage proportional to the n#merical inp#t Digital to Analog
convertors are fast and acc#rate and simple to program A n#meric val#e
is written to the address of the DAC and a voltage proportional to the this
val#e appears at the DAC o#tp#t pin
>. @mbedded Control Card
@mbedded technology plays a ma%or role in integrating vario#s tas&s of a
system This needs to tie #p vario#s f#nctions in a closed loop system 'n
this pro%ect a m#ltif#nction micro controller based embedded card is #sed
for real time motion control application The board gives combined feat#res
of position control,stable operation and increases the processing speed +#pto
.=
Fig#r
e >16
3#tlo
o& of
@mbe
dded
Contr
ol
Card
.=M*(
cloc&, and gives the
e$ibility to reprogram the microcontroller ac!cording to the motion control
need F#rther, this board gives the facility to implement the advanced
control techni)#es for motion control applications
The board consists of a microcontroller connected to fo#r Digital to Ana!
log channels,two of which are #sed in o#r pro%ect for the position control
of tail rotor and the main rotor A 1. channel Analog to Digital converter
is also interfaced to facilitate the microcontroller to comm#nicate between
analog and digital world The microcontroller is not only responsible for
comm#nicating between ;C,itself b#t also sends the necessary commands to
each DAC and ADC for con"g#ration and proper operation The board can
be #tili(ed in any application re)#iring m#ltidimensional control with high
speed,and good precision The motion control is designed in this pro%ect to
implement position control mode The codes for chec&ing the f#nctionality
of the two DAC channels and ADC are written in C #sing /eil software
The motivation to #se this m#ltif#nction board is6
" To combine the best feat#res of all the available motion control tech!
ni)#es
" To ma&e the system response fast as the board operates at .=>H?M*(
crystal fre)#ency
>- *ardware Section
" The card consists of following ma%or components6
.>
" The card consists of following ma%or components6
" ATI7C>. series microcontroller
" 8I88 Digital to Analog Converter +I bits,
" AD>H= Analog to Digital Converter +1. bits,
" -./ @$ternal Random Access Memory
" MA9.-. +for serial comm#nication,
" H=<S-H- +for m#ltiple$ing and address latching,
" Foltage reg#lator 'C
" <MH=1 3perational Ampli"er
There are fo#r loops in the system o#t of which we #sed two in o#r
pro%ect @ach loop interconnects ;C, microcontroller, and DAC in
feedforward path and ADC, microcontroller and ;C in feedbac& path,
along with some ad!ditional circ#itries The microcontroller is connected
with the ;C thro#gh serial port #sing a RS!.-. standard conversion 'C
MA9!.-. with f#ll in!terr#pt based con"g#ration to ma&e the board
reliable for real time control scheme
The serial comm#nication is chosen for the following advantages over the
parallel comm#nication6 AUB
1 Serial cables can be longer than parallel cables This is beca#se of the
standard voltage levels representing the logic levels +!- to !.> for logic 1
and D- to D.> for logic level 8,, which are m#ch higher than standards
for the parallel comm#nication +D>F for logic 1 and 8F for logic 8,
. The control card #ses ATI7C>>ED microcontroller which has b#ilt in
serial interface and can provide and receive serial data with only two
pins for transmission and reception as compared to parallel connections
that re)#ire eight pins for data transmission
>= Digital to Analog Conversion
The motor drives has a feed forward path and a feedbac& path Feed forward
path consists of DAC8I88, operational ampli"ed <MH=1 and c#rrent
ampli"er <M1. to convert the digital command inp#t of the motor into
corresponding
.?
Fig#re
>.6
Connecti
ons of
DAC1
with
microcon
troller
analog
voltage
s to be
applied
on the
inp#t of the motor to be controlled Feedbac& path consists of a 1.
channel Analog to digital Converter which provides c#rrent position of the
two rotors thro#gh a potentiometer
Digital to Analog Convertor is #sed to convert a binary word to a proportion
c#rrent or voltage The I!bit DAC8I88 has been #sed for this p#rpose in
the embedded card A negative power s#pply of !1.F is attached for proper
operation of DAC8I88 This !1.F s#pply is attached with F@@ FR@F!
pin of DAC8I88 is attached with gro#nd thro#gh a resistor as speci"ed in
DAC8I88 datasheet FR@F pin is attached with D18F s#pply thro#gh a
resistor This means that o#tp#t of digital to analog convertor can increase
from 8F to 18F only The reference voltage can be changed if re)#ired
The C lang#age code for the DAC operation is given in Appendi$ A-
>> C#rrent Ampli"cation
As the o#tp#t of <MH=1 3perational Ampli"er reaches a val#e of 7>F then
d#e to the oset it will sat#rate at this val#e This implies even if yo# in!
crease the inp#t voltage at the DAC, o#tp#t of op!amp will not increase
anymore So, the circ#it can be safely #sed to vary o#tp#t voltage from 8v
to 7>F for reference voltage C D18F
The c#rrent driving capability of <MH=1 op!amp is >88mA The motors at!
tached to the two rotors draws nearly 1>A c#rrent so an additional power
.H
Fig#re >-6 Connections of
<M1. power op!amp
ampli"er is connected to amplify the
o#tp#t c#rrent of the <MH=1
>? Control Algorithms
.I
Fig
#re
>=6
Flo
w
Cha
rt of
Con
trol
Alg
orit
hm
.7
Fig#re
>>6
Flow
Chart of
Timer
'nterr#p
t
Algorith
m
-8
Chapter ?
C32C<4S'32
Twin rotor control system is highly nonlinear system and )#ite often #sed in
control system lab to demonstrate the yaw and pitch motion of a helicopter
The wor& scope in this pro%ect covers modeling, lineari(ation of the system,
design of state space controller, fabrication of the hardware and design of
control cards
?1 Testing of the System
" 't involves testing the embedded control card to chec& if all the
mod#les are f#nctional #sing the codes developed
" The card was tested for serial comm#nication, Digital to Analog Con!
version +DAC,, and Analog to Digital Conversion +ADC,
" A simple test code for toggling was #sed to chec& the ports of the
microcontroller
" 'nitially, the mod#les were sim#lated on ;R3T@4S #sing the codes
which ens#red the viability of the code and the design
" <M1. operational ampli"er is tested by ma&ing its connections with
the two motors
-1
?. Controller Designing in C!lang#age
F#ll order state observer was designed for the system in C lang#age
The algorithm was implemented on the real system 't was able to trac& the
pitch or yaw reference inp#t speci"ed in the code b#t the system response
was not satisfactory for a changed reference inp#t sent by ;C thro#gh serial
port
-.
Appendi$ A
A1 Appendi$ 1
SN S f#nction M!file
f#nction Asys,$8,str,tsB C sf#nction+t,$,#,flag,
2o3fStatesC?L
switch flag,
NNNNNNNNNNNNNNNNNN
N 'nitiali(ation N
NNNNNNNNNNNNNNNNNN
case 8,
Asys,$8,str,tsBCmdl'nitiali(eSi(es+2o3fStates,L
NNNNNNNNNNNNNNN
N Derivatives N
NNNNNNNNNNNNNNN
case 1,
sysCmdlDerivatives+t,$,#,L
NNNNNNNNNN
N 4pdate N
NNNNNNNNNN
case .,
sysCmdl4pdate+t,$,#,L
NNNNNNNNNNN
N 3#tp#ts N
NNNNNNNNNNN
case -,
sysCmdl3#tp#ts+t,$,#,L
NNNNNNNNNNNNNNNNNNNNNNN
N 5etTime3f2e$tFar*it N
NNNNNNNNNNNNNNNNNNNNNNN
case =,
sysCmdl5etTime3f2e$tFar*it+t,$,#,L
--
NNNNNNNNNNNNN
N Terminate N
NNNNNNNNNNNNN
case 7,
sysCmdlTerminate+t,$,#,L
NNNNNNNNNNNNNNNNNNNN
N 4ne$pected flags N
NNNNNNNNNNNNNNNNNNNN
otherwise
error+A04nhandled flag C 0,n#m.str+flag,B,L
end
N end sf#n"s#b
f#nction Asys,$8,str,tsBCmdl'nitiali(eSi(es+2o3fStates,
si(es C simsi(esL
si(es2#mContStates C ?L
si(es2#mDiscStates C 8L
si(es2#m3#tp#ts C ?L
si(es2#m'np#ts C .L
si(esDirFeedthro#gh C 8L
si(es2#mSampleTimes C 1L N at least one sample time is needed
sys C simsi(es+si(es,L
N initiali(e the initial conditions
N
#18C1L#.8C1L
gC7IL
m1C8-H7.L
m.C81H-7L Nmass of Tail rotor
N++finsDmotorD+baseDfasteners,,
mwC81>IL
mlC8.I8LNMass of lever arm
N All masses are in grams
N<C1H1H>D1H7HL
lwC8878L Ndistance from pivot to weight
l1C8177>LNdistance from pivot to a$le of
Nmain rotor
l.C81H=-L Ndistance from pivot to a$le of Tail
Nmotor
Nh1CH.?HQ1888L Nhori(ontal distance from vertical a$le to lever bar
hC88.7IL Nvertical distance from hori(ontal a$le to lever bar
mCm1Dm.DmwDmlL Ntotal mass of lever bar assembly
NlwC+mlO+l1!l.,Q.Dm1Ol1!m.Ol.,QmwL
lcC+mlO+l1!l.,D+m1Ol1,!+m.Ol.,!+mwOlw,,QmL Ncenter of mass of lever bar
K<C+mlQ-,O++l1P-Dl.P-,Q+l1Dl.,,Dm1Ol1P.Dm.Ol.P.DmwOlwP.L
KAC8L
-=
T8Catan+lcQh,L N@)#ilibri#m point of the lever bar
Td8C8L
F8C1L
Fd8C8L
#CA1 1BL
T1C11LT.C8--L
&1C881L&.C881-7L
w18C#18Q&1L
w.8C#.8Q&.L
$8CAF8 Fd8 T8 Td8 w18 w.8B0L
str C ABL Nstr is always an empty matri$
ts C A8 8BL Ninitiali(e the array of sample times
N end mdl'nitiali(eSi(es
NDescription6 Calc#lation of the model0s CT state derivatives
f#nction sysCmdlDerivatives+t,$,#,
gC7IL
m1C8-H7.L
m.C81H-7L Nmass of Tail rotor
N++finsDmotorD+baseDfasteners,,
mwC81>IL
mlC8.I8LNMass of lever arm
N All masses are in grams
N<C1H1H>D1H7HL
lwC8878L Ndistance from pivot to weight
l1C8177>LNdistance from pivot to a$le of
Nmain rotor
l.C81H=-L Ndistance from pivot to a$le of Tail
Nmotor
Nh1CH.?HQ1888L Nhori(ontal distance from vertical a$le to lever bar
hC88.7IL Nvertical distance from hori(ontal a$le to lever bar
mCm1Dm.DmwDmlL Ntotal mass of lever bar assembly
NlwC+mlO+l1!l.,Q.Dm1Ol1!m.Ol.,QmwL
lcC+mlO+l1!l.,D+m1Ol1,!+m.Ol.,!+mwOlw,,QmL Ncenter of mass of lever bar
K<C+mlQ-,O++l1P-Dl.P-,Q+l1Dl.,,Dm1Ol1P.Dm.Ol.P.DmwOlwP.L
KAC8L
T1C11LT.C8--L
&1C881L&.C881-7L
FC$+1,L Nstates
FdC$+.,L
TC$+-,L
TdC$+=,L
w1C$+>,L
w.C$+?,L
->
scCsin+T,Ocos+T,L
c.s.C+cos+T,,P.!+sin+T,,P.L
DCAK<Ocos+T,P.DmOhP.Osin+T,P.!.OmOhOlcOscDKA, 8
8, +K<DmOhP.,BL
5CA 8
mOgO+lcOcos+T,!hOsin+T,,BL
Fd.Td.CA!+K<OscDmOhOlcOc.s.!mOhP.Osc,O+Td,,!+K<OscDmOhOlcOc.s.!mOhP.Osc,O+Fd,
+K<OscDmOhOlcOc.s.!mOhP.Osc,O+Fd,,8BL
MFC=?.7.O18P!>L
M*C.H777O18P!>L
TFC1.>H.O18P!>L
T*CH8H7-O18P!>L
Ta#CAM*Ol1Ow1P.Ocos+T,!T*Ol.Ow.P.Ocos+T,
!MFOl1Ow1P.DTFOl.Ow.P. BL
FricCA!888.OFdL!888.OTdBL Nfriction of potentiometer
FddTddCinv+D,O+Ta#DFric!Fd.Td.OAFd TdB0!5,L
FddCFddTdd+1,6,L
TddCFddTdd+.,6,L
w1dC!w1QT1D#+1,Q+&1OT1,L
w.dC!w.QT.D#+.,Q+&.OT.,L
sysCAFd
Fdd
Td
Tdd
w1d
w.dBL
N end mdlDerivatives
f#nction sysCmdl4pdate+t,$,#,
sys C ABL
N end mdl4pdate
f#nction sysCmdl3#tp#ts+t,$,#,
sys C A$+1,L$+.,L$+-,L$+=,L$+>,L$+?,BL
SNend mdl3#tp#ts
SN Description6Calc#late the time of the ne$t sample
SN time For systems with non!#niform sampling
f#nction sysCmdl5etTime3f2e$tFar*it+t,$,#,
sys C ABL
SN end mdl5etTime3f2e$tFar*it
f#nction sysCmdlTerminate+t,$,#,
sys C ABL
SN end mdlTerminate
-?
A. Appendi$ .
SN To find the linear model of the system
A$8,#8,y8,d$8BCtrim+0sf#ncbloc&0,
AA,B,C,DBClinmod+0sf#ncbloc&0,$8,#8,
-H
A- Appenid$ -
SN C lang#age program for the two DAC0s
Vincl#de WR@5>1hX
Vincl#de WstdiohX
#nsigned char DACdata1L
#nsigned char DACdata.L
#nsigned char tempL
char $data ODAC1C8$8881L
char $data ODAC.C8$888.L
void serial"'SR+void, interr#pt =
Y
if+R'CC1,
Y
R'C8L
tempCSB4FL
SB4FCtempL
if +tempCC0D0,
Y
DACdata1DC>8L
ODAC1CDACdata1L
DACdata.DC>8L
ODAC.CDACdata.L
;1CDACdata1L
;.CDACdata.L
Z
Z
else
Y
T'C8L
Z
Z
void main+void,
Y
DACdata1C8L
DACdata.C8L
ODAC1CDACdata1L
ODAC.CDACdata.L
-I
;1C89>>L
TM3D C 8$.8L
ACCC;C32[8$I8L
;C32CACCL
T*1C!1-L
SC32C8$>8L
'@C8$78L
TR1C1L
T*8C8$I8L
T<8C8L
TR8C1L
SB4FC0S0L
while+1,
Y
Z
Z
A= Appendi$ =
SNC"<A254A5@ ;R35RAM F3R 3BS@RF@R D@S'52
Vincl#deWATI79>1*X
Vincl#deWintrinshX
Vincl#deWmathhX
Vincl#deWstdiohX
void 'nitiali(e+void,L
void Find"<oad"Ts+void,L
char Read+char O,L
void Erite+charO,char,L
void Read"ADC+void,L
char DAC"C@+float,L
char data Ts"= "at" 8$-8L QQ ;rogrameable Sample Time +Ts, in days QQ
char data Ts"- "at" 8$-1L
char data Ts". "at" 8$-.L
char data Ts"1 "at" 8$--L
float data Ts "at" 8$-8L
char data ADC"*"Fal "at" 8$=?L QQ ADC val#e
char data ADC"<"Fal "at" 8$=HL
int data ADC"Fal "at" 8$=?L
char $data ODAC8C8$8881L
char $data ODAC1C8$888.L
-7
char $data OADC"CC8$888>L
char $data OADC"<C8$888?L
char $data OADC"*C8$888HL
char $data ORAM"-./C8$I888L
char $data Optr"rL
#nsigned int SamplesC8L QQ 2o of samples to save and then send
float a"DACL
float b"DACL
float a"ADCL
float b"ADCL
char ADC"C"BL
QQ ;rogrameable filter parameters QQ
float AA?BA?BCYY 18,88>>,8888.,88,88,88Z,
Y88,87I8-,888?-,8888I,88887,!8881=Z,
Y88,88,877-H,88>=I,88,88Z,
Y88,8888?,!8..H1,87H.-,!8881H,8888-Z,
Y88,88,88,88,87I7,88Z,Y88,88,88,88,88,87HI8ZZL
float bA?BA.BCYY8888881I,!888888??Z,Y8881,!8888=Z,
Y!888888-=, 8888881>Z,Y!8888.,88881Z,
Y8.1.-,88Z,Y88,8=778ZZL
float CA.BA?BCYY18,88,88,88,88,88Z,Y88,88,18,88,88,88ZZL
float $data 9dot"hatA?BCY88,88,88,88,88,88Z L
float $data 9"hatA?BCY88,88,88,88,88,88ZL
float $data /A.BA?BC YY8>-87,1-?7.,88->H,888.8,87>>-,!888-IZ,
Y!8I=H>,!.1HH8,!888-H,!88>.=,!88871,87?-8ZZL
float $data /"eA?BA.BCYY1I,!81Z,Y1>I,!8?Z,Y88,17Z,
Y8.,1I?Z,Y!.H.?,!1..>HZ,Y!HI11,!=1>IZZL
float $data /"eCA?BA?BL
float $data A"newA?BA?BL
float $data A"nnA?BL QO#sed for +A!/"eOC,O9"hat OQ
float $data b#A?BL QO BO+signal to the plant, matri$ OQ
float $data /"e:A?BL QO+observer gain matri$ O o#tp#t, matri$OQ
float $data :A.BCY88,88ZL QOTwo o#tp#t of the plant OQ
float $data act#ateA.BL QO 4sed for, Reference min#s o#tp#t +:, OQ
float $data Set;ointA.BCY88,88ZL QO Set point for phi and theta OQ
float $data act#ate.A.BCY88,88ZL QO Signal to the plant OQ
float $data #A.BCY88,88ZL QO #sed for /O9"hat OQ
float $data S;8,S;1L QO For conversion of angle to volts OQ
float $data g1,g.L
float $data piC-1=1>7L
=8
float $data /"Fi,/"ThetaL
char bdata flagsC8L
sbit ADC"send"flag C flagsP8L
sbit save"flag C flagsP1L
sbit Samples"send"flag C flagsP.L
sbit 5o"flag C flagsP-L
sbit ADC"DAC"Cali C flagsP?L
char bdata ;C32"setL QQ To do#ble the BR
sbit SM3D"8 C ;C32"setPHL
char Temp,Temp1L
#nsigned int nC8L
#nsigned char i,%,a,c,(L
char $data OptrL
#nsigned char DAC"Fal1,DAC"Fal.L
QQ !!!!!!!!!!!!!!QQ
QQ Main F42CT'32 QQ
QQ !!!!!!!!!!!!!!QQ
void main+void,Y
Ts C 881L
S; C 8$HFL QQ To start the stac& memmory from I8* +in idata,
'nitiali(e+,L
ADC"C"B C 8$I8L QQ 4sing ADC Channel V 8
QQQQ For step response lets ta&e the calibration e)#ation as
QQQQQQQQQQQQQQ
while+1,Y
if+ADC"DAC"Cali,Y
for+nC.888LnX8Ln!!,L QQ 1ms delay so that the DAC val#e stabili(ed on ADC iQp +'mportant,
ADC"DAC"Cali C 8L
Read"ADC+,L
SB4F C ADC"<"FalL QQQ For float transfer
ADC"send"flag C 1L QQQ For float transfer
Z
Z
Z
QQ !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!QQ
=1
QQ Timer 8 'nterr#pt +To generate Ts, QQ
QQ !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!QQ
void Timer"8 +void, interr#pt 1
Y
TR8 C 8L QQ Reload sample timer
T*8 C 8$AFL
T<8 C 8$FFL
TR8 C 1L
T8 C MT8L QQ Show *alf the Fre)#ency of the sample timer
Read"ADC+,L QQQQ Read Data from ADC QQQQQQQQQQQQQ
if+5o"flag,Y
QQ Callibration @)n of ADC
QQ : C 888=7 O ADC"Fal ! 7787?L
:A8B C a"ADC O ADC"Fal D b"ADCL
:A1B C a"ADC O ADC"Fal D b"ADCL
S;8C/"FiO+Set;ointA8B!pi,D17L QO Convert *ori(ontal Set point from
radians to volts OQ
/"FiC+17D1.,Q+.8Opi,L
/"ThetaC!188QpiL
S;1C/"Theta O +Set;ointA1B!piQ.8,!>8L QO Convert Fertical Set point from
radians to volts O
act#ateA8BC+S;8!:A8B,L QO Reference phi min#s o#tp#t OQ
act#ateA1BC+S;1!:A1B,L QO Reference theta min#s o#tp#t OQ
g1Cact#ateA8BO/A8BA8BDact#ateA1BO/A8BA.BL QO gain matri$ in the forward
path, #sed for trac&ing OQ
g.Cact#ateA8BO/A1BA8BDact#ateA1BO/A1BA.BL
for+iC8LiWC1LiDD,Y
for+%C8L%WC>L%DD,
#AiBC#AiBD+/AiBA%BO9"hatA%B,L
Z
act#ate.A8BC+g1!#A8B,L QO Act#ator signal to plant OQ
act#ate.A1BC+g.!#A1B,L QO Ac#ator signal to plant OQ
QO Calc#late BO# OQ
for+iC8LiWC>LiDD,Y
for+%C8L%WC1L%DD,
b#A%BDC+bAiBA%BOact#ate.A%B,L
Z
QOOOOO Calc#lation of /"e:C/"eO: OOOOOOQ
for+iC8LiWC>LiDD,
Yfor+%C8L%WC1L%DD,
/"e:AiBDC+/"eAiBA%BO:A%B,LZ
QOOOOO Calc#lation of /"eCC/"eOC OOOOOOQ
=.
for+iC8LiWC>LiDD,
Yfor+%C8L%WC1L%DD,
Yfor+(C8L(WC1L(DD,
/"eCAiBA%BDC+/"eAiBA(BOCA(BA%B,LZZ
for+iC8LiW-LiDD,
for+%C8L%W-L%DD,
A"newAiBA%BCAAiBA%B!/"eCAiBA%BL
QOOOOO Calc#lation of +5!<OC,O9"hat OOOOOQ
for+iC8LiWC>LiDD,Y
for+%C8L%WC>L%DD,
A"nnAiBDC+A"newAiBA%BO9"hatA%B,L
Z
for+aC8LaWC>LaDD,
9dot"hatAaBC+A"nnAaBDb#AaBD/"e:AaB,L
for+cC8LcWC>LcDD,
9"hatAcBC9dot"hatAcBL QO #pdate the states OQ
QQ Callibration @)#ation 3f Dac and Sat#ration limit
act#ate.A8B C +int,+a"DAC O DAC"Fal1 D b"DAC,L
if+act#ate.A8B X .>>, QQ Apply sat#ration
DAC"Fal1 C .>>L
else if+act#ate.A8B W 8,
DAC"Fal1 C 8L
else if+act#ate.A8B W .>> \\ act#ate.A8B X 8, QQ 'f 5lobal \ Rel";os"int 0s are enable
DAC"Fal1 C +char,act#ate.A8BL
Erite+DAC8,DAC"Fal1,L
act#ate.A1B C +int,+a"DAC O DAC"Fal. D b"DAC,L
if+act#ate.A1B X .>>, QQ Apply sat#ration
DAC"Fal. C .>>L
else if+act#ate.A1B W 8,
DAC"Fal. C 8L
else if+act#ate.A1B W .>> \\ act#ate.A1B X 8, QQ 'f 5lobal \ Rel";os"int 0s are enable
DAC"Fal. C +char,act#ate.A1BL
Erite+DAC1,DAC"Fal.,L
QQ 4pdating val#es for the ne$t itteration
for+(C8L(WC>L(DD,Y
9dot"hatA(BC88L
A"nnA(BC88L
/"e:A(BC88L
b#A(BC88L
=-
Z
Z
QQ Save sample
if+save"flag,Y
Erite+ADC"<,ADC"<"Fal,L
Erite+ADC"*,ADC"*"Fal,L
if+DDn CC Samples,Y
n C 8L
TR8 C 8L
ptr"r C RAM"-./L QQ 'nitiali(e two pinters at -./ RAM
SB4F C Optr"rDDL
Samples"send"flag C 1L
save"flag C 8L
QQ Samples C .8OSamplesL QQ BQC each sample has two val#es
Z
Z
Z
QQ !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!QQ
QQ Serial 'nterr#pt +Attached to ;C, QQ
QQ !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!QQ
void Serial"int +void, interr#pt =Y
if+T'CC1,Y QQ 'f transmit interr#pt occ#r
T' C 8L
if+ADC"send"flag,Y
ADC"send"flag C 8L
QQ if+ADC"*"Fal CC 8,
QQ ADC"*"Fal C 0B0L QQ Send 0B0 C Blan&, if high byte is 8
SB4F C ADC"*"FalL
ZQQ end of ADC"send"flag0s if
Z
else
Y
R' C 8L QQ 'f Receive interr#pt occ#r
if +SB4FCC0S0,
Y
Erite+DAC8,DAC"Fal1,L
Erite+DAC1,DAC"Fal.,L
TF8C1L
==
Z
Z
Z
QQ !!!!!!!!!!!!!!!!!!!!!QQ
QQ 'nitiali(ing Devices QQ
QQ !!!!!!!!!!!!!!!!!!!!!QQ
void 'nitiali(e+void,
Y
SC32 C 8$>8L
TM3D C 8$.1L
QQT*1 C !-L
T*1C!1-L
QQ Mode 1 +I bit 4ART with variable BR mode,
QQ Timer 1 as I bit a#to reload mode and Timer 8 as 1?!bit mode
QQ Setting timer1 for -I=88 BR with smod C 1 +.=>H? M*( 9tal,
;C32"set C ;C32L QQ setting Smod C 1
SM3D"8 C 1L
;C32 C ;C32"setL
TR1 C 1L QQ Start generating BR
'@ C 8$7.L QQ @nable Serial, Timer 8 \ 5lobal interr#pts
'; C 8$18L QQ Serial port has a higher priority M at the start
'T8 C 1L
'T1 C 1L
SB4FC 0S0L
Erite+DAC8,8$HF,L QQ 'nitiali(e DAC to (ero
Erite+DAC1,8$HF,L
Z
QQ !!!!!!!!!!!!!!!QQ
QQ Read F#nction QQ
QQ !!!!!!!!!!!!!!!QQ
char Read+char Oadd,
Y
ptr C addL
ret#rn OptrL
Z
QQ !!!!!!!!!!!!!!!QQ
QQ Erite F#nction QQ
QQ !!!!!!!!!!!!!!!QQ
void Erite+char Oadd,char val,Y
=>
ptr C addL
Optr C valL
Z
QQ !!!!!!!!!!!!!!!!!!!QQ
QQ Read ADC F#nction QQ
QQ !!!!!!!!!!!!!!!!!!!QQ
void Read"ADC+void,Y
Temp1 C ADC"C"B \ 8$FHL
Erite+ADC"C,Temp1,L
for+iC8LiW>8LiDD,L QQ converge delay +i m#st be WC .>>,
QQ Select chamnnel 8 and generate S3C
Temp1 C ADC"C"B [ 8$8IL QQ Reading ADC +last selected channel of AD5=8I,
Erite+ADC"C,Temp1,L
QQ Read sample
ADC"<"Fal C Read+ADC"<,L
ADC"*"Fal C Read+ADC"*,L
if+ADC"*"Fal CC 8,
ADC"*"Fal C 0B0L
:A8B C a"ADC O ADC"Fal D b"ADCL
:A1B C a"ADC O ADC"Fal D b"ADCL QQ Send 0B0 C Blan&, if high byte is 8
Z
QQ !!!!!!!!!!!!!!!!!!!!QQ
QQ Erite DAC F#nction QQ
QQ !!!!!!!!!!!!!!!!!!!!QQ
char DAC"C@+float ff,Y
#nsigned char chL
ff C ff O a"DAC D b"DACL
if+ffX.>>,
ch C .>>L
else if+ffW8,
ch C 8L
else
ch C +#nsigned char,ffL
ret#rn chL
Z
=?
Bibliography
A1B Magn#s 5afvert, Modelling of the @T* *elicopter <aboratory ;rocess, Depart!
ment of A#tomatic Control <#nd 'nstit#te of Technology, 2ovember .881 4R<6
http6QQwwwcontrollthseQdoc#mentsQ.881Qgaf81tpdf
A.B /ats#hi&o 3gata, Modern Control @ngineering, Forth edition, ;earson @d#cation 'nc 'ndia, .88-
A-B Testing and Comparison of Dierent Control Algorithms on M'M3 Systems, Ga&ir#llah,.881
=H
Bibliography
=I