780c5906babfcecc57a00fa04272df9c6d2a282d
[scilab.git] / scilab / modules / cacsd / demos / pendulum / pendule.dem
1 //
2 // Scilab ( http://www.scilab.org/ ) - This file is part of Scilab
3 // Copyright (C) ????-2008 - INRIA
4 //
5 // This file is distributed under the same license as the Scilab package.
6 //
7
8 mode(-1)
9 path=get_absolute_file_path('pendule.dem');
10
11 getf(path+'simulation.sci')
12 getf(path+'graphics.sci')
13
14 my_handle             = scf(100001);
15 clf(my_handle,"reset");
16
17 xselect();
18 wdim=xget('wdim')
19 //mode(1)
20
21  dpnd()
22 //
23 // equations 
24 //----------
25 //state =[x x' theta theta']  
26 //
27  mb=0.1;mc=1;l=0.3;m=4*mc+mb; //constants
28 //
29 x_message(['Open loop simulation'
30            ' '
31            ' y0 = [0;0;0.1;0]; //initial state [x x'' theta theta'']'
32            ' t = 0.03*(1:180); //observation dates'
33            ' y = ode(y0,0,0.03*(1:180),ivpd); //differential equation integration'
34            ' //Display'
35            ' P = initialize_display(y0(1),y0(3))'
36            ' for k=1:size(y,2), set_pendulum(P,y(1,k),y(3,k));end'])
37 //
38  y0=[0;0;0.1;0];
39  y=ode(y0,0,0.03*(1:180),ivpd);
40
41  P=initialize_display(y0(1),y0(3));
42  for k=1:size(y,2), set_pendulum(P,y(1,k),y(3,k));end
43
44 //
45 x_message(['Linearization'
46            ' '
47            ' x0=[0;0;0;0];u0=0;'
48            ' [f,g,h,j]=lin(pendu,x0,u0);'
49            ' pe=syslin(''c'',f,g,h,j);'
50            ' // display of the linear system'
51            ' ssprint(pe)'])
52
53 //
54 mode(1)
55  x0=[0;0;0;0];u0=0;
56  [f,g,h,j]=lin(pendu,x0,u0);
57  pe=syslin('c',f,g,h,j);
58  ssprint(pe)
59 mode(-1)
60  
61 //
62 x_message(['Checking the result'
63            ' //comparison with linear model computed by hand';
64            ''
65            ' f1=[0 1        0                0'
66            '     0 0    -3*mb*9.81/m         0'
67            '     0 0        0                1'
68            '     0 0  6*(mb+mc)*9.81/(m*l)   0];'
69            ' '
70            ' g1=[0 ; 4/m ; 0 ; -6/(m*l)];'
71            ' '
72            ' h1=[1 0 0 0'
73            '     0 0 1 0];'
74            ' '
75            ' err=norm(f-f1,1)+norm(g-g1,1)+norm(h-h1,1)+norm(j,1)'])
76
77  
78 //
79 mode(1)
80  f1=[0 1        0             0
81     0 0    -3*mb*9.81/m         0
82     0 0        0             1
83     0 0  6*(mb+mc)*9.81/(m*l)   0];
84  g1=[0 ; 4/m ; 0 ; -6/(m*l)];
85  h1=[1 0 0 0
86      0 0 1 0];
87  err=norm(f-f1,1)+norm(g-g1,1)+norm(h-h1,1)+norm(j,1)
88  mode(-1)
89  
90 x_message(['Linear system properties analysis'
91            ' spec(f) //stability (unstable system !)'
92            ' n=contr(f,g) //controlability'
93            ' '
94            ' //observability'
95            ' m1=contr(f'',h(1,:)'') '
96            ' [m2,t]=contr(f'',h(2,:)'')'])
97 //---------
98 mode(1)
99  spec(f) //stability (unstable system !)
100  n=contr(f,g) //controlability
101
102  //observability
103  m1=contr(f',h(1,:)') 
104  [m2,t]=contr(f',h(2,:)')
105  mode(-1)
106 //
107 x_message(['Synthesis of a stabilizing controller using poles placement'
108            ' '
109            '// only x and theta are observed  : contruction of an observer'
110            '// to estimate the state : z''=(f-k*h)*z+k*y+g*u'
111            ' to=0.1; ' 
112            ' k=ppol(f'',h'',-ones(4,1)/to)''  //observer gain'
113            '// compute the compensator gain'
114            ' kr=ppol(f,g,-ones(4,1)/to)'])
115
116 //-------------------------------------------------
117 //
118 //pole placement technique
119 //only x and theta are observed  : contruction of an observer
120 //to estimate the state : z'=(f-k*h)*z+k*y+g*u
121 //
122  to=0.1;  //
123  k=ppol(f',h',-ones(4,1)/to)'  //observer gain
124 //
125 //verification
126 //
127 // norm( poly(f-k*h,'z')-poly(-ones(4,1)/to,'z'))
128 //
129  kr=ppol(f,g,-ones(4,1)/to)  //compensator gain
130  
131 //
132 x_message(['Build full linear system  pendulum-observer-compensator'
133            ' '
134            ' ft=[f-g*kr            -g*kr'
135            '     0*f               f-k*h]'
136            ' '
137            ' gt=[g;0*g];'
138            ' ht=[h,0*h];'
139            ''
140            ' pr=syslin(''c'',ft,gt,ht);'
141            ''
142            '//Check the closed loop dynamic'
143            ' spec(pr.A)'
144            ' '
145            '//transfer matrix representation'
146            ' hr=clean(ss2tf(pr),1.d-10)'
147            ' '
148            '//frequency analysis'
149            ' black(pr,0.01,100,[''position'',''theta''])'
150            ' g_margin(pr(1,1))'])
151
152 //---------------------------------------------
153 //
154 //state: [x x-z]
155 //
156 mode(1)
157  ft=[f-g*kr            -g*kr
158       0*f               f-k*h];
159  gt=[g;0*g];
160  ht=[h,0*h];
161  pr=syslin('c',ft,gt,ht);
162
163 // closed loop dynamics:
164  spec(pr(2))
165 //transfer matrix representation
166  hr=clean(ss2tf(pr),1.d-10)
167  
168  
169  //frequency analysis
170  clf()
171  black(pr,0.01,100,['position','theta'])
172  g_margin(pr(1,1))
173  mode(-1)
174  
175  
176 //
177 x_message(['Sampled system'
178            ' '
179            ' t=to/5; //sampling period'
180            ' prd=dscr(pr,t); //discrete model'
181            ' spec(prd.A) //poles of the discrete model'])
182
183 //---------------
184 //
185 mode(1)
186  t=to/5;
187  prd=dscr(pr,t);
188  spec(prd(2))
189 mode(-1)
190 //
191 x_message(['Impulse response'
192            ' '
193            ' x0=[0;0;0;0;0;0;0;0]; //initial state'
194            ' u(1,180)=0;u(1,1)=1; //impulse'
195            ' y=flts(u,prd,x0);    //siscrete system simulation'
196            ' draw1()'])
197
198 //-----------------
199 //
200 mode(1)
201  x0=[0;0;0;0;0;0;0;0];
202  u(1,180)=0;u(1,1)=1;
203  y=flts(u,prd,x0);
204
205  draw1()
206 mode(-1)
207 //
208 x_message(['Compensation of the non linear system with'
209            'linear regulator'
210            ''
211            ' t0=0;t1=t*(1:125);'
212            ' x0=[0 0 0.4 0   0 0 0 0]'';  // initial state'
213            ' yd=ode(x0,t0,t1,regu); //integration of differential equation'
214            ' draw2()'])
215 ;
216 //--------------------------------------
217 //
218 //simulation
219 //
220 mode(1)
221  t0=0;t1=t*(1:125);
222  x0=[0 0 0.4 0   0 0 0 0]';   //
223  yd=ode(x0,t0,t1,regu);
224  draw2()
225 mode(-1) 
226  x_message('The end')