function Xd=simula(t,X) x=X(:,1);dx=X(:,2:\$) if dx<>[] then [xd,dxd]=full_modela(x,dx) Xd=[xd,dxd] else Xd=full_model(X) end endfunction function [xd,dxd]=full_modela(x,dx) z=x(1);th=x(2);zd=x(3);thd=x(4); Mati=inv([M+m,m*l*cos(th);cos(th),l]); ydd=Mati*[m*l*sin(th)*thd^2+U;g*sin(th)]; zdd=ydd(1);thdd=ydd(2); xd=[zd;thd;zdd;thdd]; if argn(2)==2 then F=[0,m*l*thdd*sin(th)+m*l*thd^2*cos(th);0,zdd*sin(th)+g*cos(th)]; G=[0,2*m*l*thd*sin(th);0,0]; dxd=[zeros(2,2),eye(2,2);Mati*F,Mati*G]*dx; end endfunction function pendanim(anim) xset('window',1);xbasc() if ~anim then driver('Pos') xinit('pendfig') end tc=0; XX=[];TT=[];UU=[];Ts=0 xi=x0; for i=1:N U=(-1)^(i-1)*Umax;UU=[UU,U];Ts=[Ts,Ts(\$)+Ti(i)]; if Ti(i)>1d-9 then tt=tc:.001:tc+Ti(i); X=ode(xi,tc,tt(2:\$),1d-11,1d-12,simula); TT=[TT,tt(2:\$)];XX=[XX,X]; tc=tc+Ti(i); xi=X(:,\$); end end subplot(4,1,1) plot2d(TT',XX(1,:)') xgrid() subplot(4,1,2) plot2d(TT',XX(2,:)') xgrid() subplot(4,1,3) UU(\$+1)=UU(\$); plot2d2(Ts',UU') xgrid() subplot(4,1,4) plot2d(TT',XX([3,4],:)') xgrid() if ~anim then xend() end xset('window',2) xbasc() if ~anim then driver('Pos') xinit('pendfig2') end h=gcf(); nc=100; if ~anim then h.color_map=graycolormap(nc); end h.children.isoview='on'; h.children.data_bounds=[-l,-l;l,l]; xpoly([-2*l,2*l],[0,0]) //xpoly([0,0],[-2*l,2*l]) X=XX(:,1); z=X(1);th=X(2); xp=[z,z+l*sin(th)];yp=[0,l*cos(th)]; e=l/10; xe=z+[-e,-e,e,e,e];ye=[-e,e,e,-e,-e]; if anim then xfpoly(xe,ye);cart=gce(); xpoly(xp,yp);lnk=gce(); lnk.thickness = 5; lnk.foreground= 5; end if anim then for X=XX z=X(1);th=X(2); xe=z+[-e,-e,e,e,e]; xp=[z,z+l*sin(th)];yp=[0,l*cos(th)]; cart.data=[xe',ye']; lnk.data=[xp;yp]'; xpause(10000) end else i=nc for t=linspace(0,TT(\$),nc-1) i=i-1; j=min(find(t<=TT)); X=XX(:,j); z=X(1);th=X(2); xp=[z,z+l*sin(th)];yp=[0,l*cos(th)]; xpoly(xp,yp);lnk=gce(); lnk.foreground= i; end xend() driver('Rec') if MSDOS then comm=pathconvert(SCI+'\bin\BEpsf',%f,%f,'w') unix_g(''"'+comm+''" '+'pendfig') unix_g(''"'+comm+''" '+'pendfig2') else unix_g(SCI+'/bin/BEpsf '+'pendfig') unix_g(SCI+'/bin/BEpsf '+'pendfig2') end end endfunction