%
% File: flex.m
% Flexible beam simulation with mouse/touchpad input
% and real-time visualization
% Created by Marc Bodson. Last revised: 5/23/2025
%
% 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];
sysp1=tf(num1,den);sysd1=c2d(sysp1,dt,'zoh');
[dnum1,dden]=tfdata(sysd1,'v');dim=length(dden)-1;
ap=[zeros(dim-1,1) eye(dim-1);-fliplr(dden(2:dim+1))];
bp=[zeros(dim-1,1);1];cp1=fliplr(dnum1(2:dim+1));
sysp2=tf(num2,den);sysd2=c2d(sysp2,dt,'zoh');
[dnum2,dden]=tfdata(sysd2,'v');cp2=fliplr(dnum2(2:dim+1));
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');
xl=[-lbeam,0,lbeam];yl=[lbeam 0 lbeam];
pline=plot(xl,yl,'blue');
%
% 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                         % manual control
            scale=get(0,'MonitorPosition');
            cursor=get(0,'PointerLocation');
            xval=2*(cursor(1)-1)/(scale(1,3)-1)-1;
            imotor=-gmanual*xval;
        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