%
% File newdemo.m
% Demo for three simulation programs with mouse/touchpad input 
% and real-time visualization
% Created by Marc Bodson. Last revised: 7/20/23
%
% Adjust the maximum simulation time
tmax=30;   
% Adjust the position & size of the visualization windows (if desired)
% Format: [position from left, position from bottom, width, height]
winbbeam=[20 350 940 300];   % Ball and beam
wininv=[20 300 970 393];     % Inverted pendulum
winflex=[30 280 810 400];    % Flexible beam
% Adjust cheat variables to make the manual control more manageable
cheatinv=6;  % reduce gravity by this factor for the inverted pendulum
cheatflex=5; % slow the system down by this factor for the flexible beam
%
alldone=0;
while (alldone==0)
    % Choice of simulation ------------------------------------------------
    fprintf('Type 1 for manual control of ball and beam \n');
    fprintf('Type 2 for automatic control of ball and beam\n');
    fprintf('Type 3 for manual control of inverted pendulum\n');
    fprintf('Type 4 for automatic control of inverted pendulum\n');
    fprintf('Type 5 for manual control of flexible beam\n');
    fprintf('Type 6 for automatic control of flexible beam\n');
    testcase=input('Type ctrl-C to stop the demo.\n');
    switch testcase
        case 1;system=1;mode=1;case 2;system=1;mode=2;
        case 3;system=2;mode=1;case 4;system=2;mode=2;
        case 5;system=3;mode=1;case 6;system=3;mode=2;
        otherwise;system=0;alldone=1;
    end
    switch system
        case 1   % Ball and beam ------------------------------------------
            % Ball and beam - Model initialization
            lbeam=0.8;       % beam length (m)
            xmax=lbeam/2;    % max. ball position (m)
            thmax=pi/36;     % max. beam angle (rad)
            xball=0;         % initial ball position (m)
            vball=0;         % initial ball velocity (m/s)
            dt=0.05;         % sampling period (s)
            g=9.81;g57=g*5/7;
            % Ball and beam - Controller initialization
            switch mode
                case 1   % Manual
                    gmanual=thmax; % sensitivity of manual controller
                case 2   % Automatic PD gains
                    kp=-1.5857;kv=-0.9514;
            end
            % Ball and beam - Visualization initialization
            figure(1);
            wbeam=0.012;rball=0.008;npb=20;ipb=0:npb;vgap=0.2*rball;
            axis([-0.6*lbeam 0.6*lbeam -0.2*lbeam 0.2*lbeam]);clf
            axis([-0.6*lbeam 0.6*lbeam -0.2*lbeam 0.2*lbeam]);
            set(1,'pos',winbbeam);hold on;
            xdbeam=[lbeam/2;lbeam/2;-lbeam/2;-lbeam/2];
            ydbeam=[-wbeam/2;wbeam/2;wbeam/2;-wbeam/2];
            pbeam=fill(xdbeam,ydbeam,'green');
            xdball=rball*cos(2*pi*ipb/npb);
            ydball=rball*sin(2*pi*ipb/npb)+wbeam/2+rball+vgap;
            pball=fill(xdball,ydball,'red');
            xline1=[-0.375*lbeam;-0.375*lbeam];yline1=[-wbeam/2;wbeam/2];
            pline1=plot(xline1,yline1,'black');
            xline2=[0.375*lbeam;0.375*lbeam];yline2=[-wbeam/2;wbeam/2];
            pline2=plot(xline2,yline2,'black');
            % Ball and beam - Real-time simulation
            t=0;tm=0;nt=0;done=0;tic;
            while (done==0)
                while tm<nt;tm=toc;end;t=nt;nt=t+dt;
                % Ball and beam - Controller
                % Control signal:   theta = beam angle (rad)
                % Useful variables: xball = ball position (m)
                %   vball = ball velocity (m/s), t = time (s)
                scale=get(0,'MonitorPosition');
                cursor=get(0,'PointerLocation');
                xval=2*(cursor(1)-1)/(scale(1,3)-1)-1;
                switch mode    
                    case 1 % Manual
                        theta=-gmanual*xval;
                    case 2 % Automatic
                        xref=0.3*xval;theta=kp*(xref-xball)-kv*vball;
                end
                % Ball and beam - Model
                if (theta>thmax);theta=thmax;end
                if (theta<-thmax);theta=-thmax;end
                vball=vball-dt*g57*sin(theta);
                if and((abs(theta)<0.02),(abs(vball)<0.002))
                    vball=0;end  % pseudo-friction
                xball=xball+dt*vball;
                if (xball>xmax);xball=xmax;vball=0;end
                if (xball<-xmax);xball=-xmax;vball=0;end
                % Ball and beam - Visualization
                u=[cos(theta) -sin(theta);sin(theta) cos(theta)];
                dball=[xdball'+xball ydball']*u';xpball=dball(:,1);ypball=dball(:,2);
                set(pball,'Xdata',xpball,'Ydata',ypball);
                dbeam=[xdbeam ydbeam]*u';xpbeam=dbeam(:,1);ypbeam=dbeam(:,2);
                set(pbeam,'Xdata',xpbeam,'Ydata',ypbeam);
                dline1=[xline1 yline1]*u';xpline1=dline1(:,1);ypline1=dline1(:,2);
                set(pline1,'Xdata',xpline1+1e-6*rand,'Ydata',ypline1);% rand to avoid erase
                dline2=[xline2 yline2]*u';xpline2=dline2(:,1);ypline2=dline2(:,2);
                set(pline2,'Xdata',xpline2+1e-6*rand,'Ydata',ypline2);
                drawnow;
                %
                if (t>=tmax);done=1;end
            end
            hold off;
            % End of ball and beam
            %
        case 2   % Inverted pendulum --------------------------------------
            %
            % Inverted pendulum - Model initialization
            cheat=1;if (mode==1);cheat=cheatinv;end
            g=9.81/cheat;          % acceleration of gravity (m/s^2)
            lcg=1;lbeam=lcg*2;     % beam length (m)
            thmax=pi/6;xmax=lbeam; % max. beam angle (rad) and cart position (m)
            theta=0;xcart=0;       % initial beam angle (rad) and cart position (m)
            dt=0.05;               % sampling period (s)
            a2=3*g/(4*lcg);nump=[-a2/g 0 0];denp=[1 0 -a2];sysp=tf(nump,denp);
            [ap,bp,cp,dp]=ssdata(c2d(sysp,dt,'zoh'));f=5;
            icca=inv([cp;cp*ap]);iccb=icca*([dp;cp*bp+dp]); % zero velocity initialization
            xp=icca*[theta;theta]-iccb*xcart;xss=icca*[thmax;thmax];
            % Inverted pendulum - Contoller initialization
            switch mode
                case 1 % Manual
                    gmanual=xmax; % sensitivity of manual controller
                case 2 % Automatic
                    c=0.3391;ac =[2.1284 -0.563;2 0];bc=[1;0];cc=[-1.3536 0.6299];
                    dc=-2.4412;xc=[0;0];xreff=0;
            end
            % Inverted pendulum - Visualization initialization
            figure(1);
            wbeam=0.016*lbeam;vgap=0.005*lbeam;wtrack=2.12*lbeam;
            htrack=0.03*lbeam;wcart=0.12*lbeam;hcart=0.05*lbeam;
            axis([-0.75*wtrack 0.75*wtrack 0 htrack+hcart+1.2*lbeam]);clf
            axis([-0.75*wtrack 0.75*wtrack 0 htrack+hcart+1.2*lbeam]);
            set(1,'pos',wininv);hold on % position/size of sim. window
            xdtrack=[wtrack/2;wtrack/2;-wtrack/2;-wtrack/2];
            ydtrack=[0;htrack;htrack;0];
            ptrack=fill(xdtrack,ydtrack,'green');
            xline1=[-lcg,-lcg];yline1=[0,htrack];
            pline1=plot(xline1,yline1,'black');
            xline2=[lcg,lcg];yline2=[0,htrack];
            pline2=plot(xline2,yline2,'black');
            xdcart=[wcart/2;wcart/2;-wcart/2;-wcart/2];
            ydcart=[0;hcart;hcart;0];
            pcart=fill(xdcart,ydcart+htrack+vgap,'blue');
            xdbeam=[wbeam/2;wbeam/2;-wbeam/2;-wbeam/2];
            ydbeam=[0;lbeam;lbeam;0];
            pbeam=fill(xdbeam,ydbeam+htrack+vgap+vgap+hcart,'red');
            % Inverted pendulum - Real-time simulation
            t=0;tm=0;nt=0;done=0;tic;
            while (done==0)
                while tm<nt;tm=toc;end;t=nt;nt=t+dt;
                % Inverted pendulum - Controller
                % Control signal:   xref = reference cart position (m)
                % Useful variables: xcart = cart position (m)
                %    theta = beam angle (rad), t = time (s)
                scale=get(0,'MonitorPosition');
                cursor=get(0,'PointerLocation');
                xval=2*(cursor(1)-1)/(scale(1,3)-1)-1;
                switch mode
                    case 1   % Manual
                        xcom=gmanual*xval;
                    case 2  % Automatic
                        xref=xval;xreff=xreff+dt*c*(xref-xreff);
                        xcom=xreff-(cc*xc+dc*theta);xc=ac*xc+bc*theta;
                end
                % Inverted pendulum - Model
                xcart=xcart+dt*f*(xcom-xcart);
                if (xcart>xmax);xcart=xmax;end
                if (xcart<-xmax);xcart=-xmax;end
                theta=cp*xp+dp*xcart;xp=ap*xp+bp*xcart;
                if (theta>thmax);theta=thmax;xp=xss-iccb*xcart;end
                if (theta<-thmax);theta=-thmax;xp=-xss-iccb*xcart;end
                % Inverted pendulum - Visualization
                u=[cos(theta) sin(theta);-sin(theta) cos(theta)];
                xpcart=xdcart+xcart;ypcart=htrack+vgap+ydcart;
                dbeam=[xdbeam ydbeam]*u';
                xpbeam=xcart+dbeam(:,1);ypbeam=htrack+hcart+vgap+vgap+dbeam(:,2);
                set(pcart,'Xdata',xpcart,'Ydata',ypcart);
                set(pbeam,'Xdata',xpbeam,'Ydata',ypbeam);
                drawnow;
                %
                if (t>=tmax);done=1;end
            end
            hold off
            % End of inverted pendulum
            %
        case 3   % Flexible beam ------------------------------------------
            %
            % Flexible beam - Model initialization
            cheat=1;if (mode==1);cheat=cheatflex;end
            imax=6;       % max. motor current (A)
            dphimax=pi/6; % max. tip angle - shaft angle in visualization (rad)
            dt=0.005;     % sampl. period (s)
            kp=5.5;       % DC gain from motor to angular acceleration (rad/(s^2.A))
            dtsim=dt*cheat;
            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;
            % Flexible beam - Controller initialization
            switch mode
                case 1 % Manual
                    gmanual=imax; % sensitivity of manual controller
                case 2 % Automatic
                    af=0.9809;bf=0.1250;cf=0.1529;df=0;
                    ac=[2.1804 -0.3952 0.0476;4 0 0;0 2 0];
                    bc=[8;0;0];cc=[-7.8388 3.5483 -0.8215];dc=97.7119;
                    xf=0;xc=[0;0;0];
            end
            % Flexible beam -  Visualization initialization
            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',winflex);hold on;
            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');
            % Flexible beam - 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;
                % Flexible beam - 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)
                scale=get(0,'MonitorPosition');
                cursor=get(0,'PointerLocation');
                xval=2*(cursor(1)-1)/(scale(1,3)-1)-1;
                switch mode
                    case 1   % Manual
                        imotor=-gmanual*xval;
                    case 2   % Automatic
                        phiref=-(pi/4)*xval;
                        phireff=cf*xf+df*phiref;xf=af*xf+bf*phiref;
                        imotor=cc*xc+dc*(phireff-phi);xc=ac*xc+bc*(phireff-phi);
                end
                % Flexible beam - Model
                xp=ap*xp+bp*imotor;theta=cp1*xp;phi=cp2*xp;
                % Flexible beam - 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;
                %
                if (tsim>=tmax);done=1;end
            end
            hold off;
            % End of flexible beam
    end % End of choice of system
end % End of alldone loop