%
% File: flex.m
% Flexible beam simulation with visualization
% and joystick input.
% Created by Marc Bodson. Last revised: 04/11/06.
%
% Initialization - Flexible beam model
%
imax=6;       % max. motor current (A)
dphimax=pi/6; % max. tip angle - shaft angle in visualization (rad)
dt=0.005;     % sampl. period (s) 
dtsim=0.05;   % sampl. period for visualization (s)
kp=5.5;       % DC gain from motor to angular acceleration (rad/(s^2.A))
z11=-.07 + 18i;z12=-0.07 + 180i;z21=-300;z22=200;
z23=-120;z24=100;p1=-3+74i;p2=-3+215i;
den1=[1 -2*real(p1) abs(p1)^2];
den2=[1 -2*real(p2) abs(p2)^2];den=conv(den1,den2);
num11=[1 -2*real(z11) abs(z11)^2 ];num12=[1 -2*real(z12) abs(z12)^2 ];
num1=conv(num11,num12);num1=num1*kp*den(5)/num1(5);
num21=[1 -(z21+z22) (z21*z22)];num22=[1 -(z23+z24) (z23*z24)];
num2=conv(num21,num22);num2=num2*kp*den(5)/num2(5);den=[den 0 0];
sysp=tf({num1;num2},{den;den});
[ap,bp,cp,dp]=ssdata(c2d(sysp,dt,'zoh'));
cp1=cp(1,:);cp2=cp(2,:);
xp=zeros(6,1);theta=cp1*xp;phi=cp2*xp;
%
% Initialization - Controller
%
mode = input('Type 1 for manual control, 2 for automatic control:\n');
switch mode;
   case 1;gmanual=imax; % gain of manual controller
   case 2;flexcinit;    % initialization of automatic controller
end;
%
% Initialization - Visualization
% Adjust the position & size of the simulation window below 
% to fit the screen and to square the axes (if needed)
% Format: [position from left, position from bottom, width, height]
%
figure(1);
lbeam=1.;npb=20;ipb=0:npb;wbeam=0.02;wbeam2=wbeam/2;
axis([-1.05*lbeam 1.05*lbeam -0.05*lbeam 1.05*lbeam]);clf
axis([-1.05*lbeam 1.05*lbeam -0.05*lbeam 1.05*lbeam]);
set(1,'pos',[30 280 810 400]);hold on % position/size of sim. window
al=ipb/npb;al2=al.^2;all=al*lbeam;
xdbeam=zeros(1,2*npb+2);ydbeam=zeros(1,2*npb+2);
pbeam=fill(xdbeam,ydbeam,'red','EraseMode','background');
xl=[-lbeam,0,lbeam];yl=[lbeam 0 lbeam];
pline=plot(xl,yl,'blue','EraseMode','background');
%
% Real-time simulation
%
tm=0;t=0;nt=0;tsim=0;ntsim=0;tm;done=0;tic;
while (done==0);
   while tm<ntsim;tm=toc;end;t=nt;nt=t+dt;tsim=ntsim;ntsim=tsim+dtsim;
%
% Controller
% Control signal:   imotor, motor current (A)
% Useful variables: theta, angle of the motor shaft (rad) 
%                   phi, angle of the tip of the beam (rad)
%
switch mode;
   case 1;imotor=-gmanual*jstick; % manual control
   case 2;flexc;                  % automatic control
end;
%
% Flexible beam model
% 
xp=ap*xp+bp*imotor;theta=cp1*xp;phi=cp2*xp;
%
% Visualization
%
if (imotor>imax);imotor=imax;end;
if (imotor<-imax);imotor=-imax;end;
dphi=phi-theta;
if (dphi>dphimax);dphi=dphimax;end;
if (dphi<-dphimax);dphi=-dphimax;end;
set(pline,'Xdata',xl+1e-6*rand,'Ydata',yl); % rand to avoid erase
xcb=all-(2*dphi^2/3)*all.*al2;ycb=dphi*all.*al;
scup=(1-wbeam*dphi/lbeam);sclo=(1+wbeam*dphi/lbeam);
xup=xcb*scup;yup=ycb*scup+wbeam2;xlo=xcb*sclo;ylo=ycb*sclo-wbeam2;
xdbeam=[xlo fliplr(xup)];ydbeam=[ylo fliplr(yup)];
u=[-sin(theta) -cos(theta);cos(theta) -sin(theta)];
dbeam=[xdbeam' ydbeam']*u';xpbeam=dbeam(:,1);ypbeam=dbeam(:,2);
set(pbeam,'Xdata',xpbeam,'Ydata',ypbeam);drawnow;
%
end
hold off