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