%
% File: invpend.m
% Inverted pendulum simulation with mouse/touchpad input
% and real-time visualization
% Created by Marc Bodson. Last revised: 5/23/25
%
% Initialization - Inverted pendulum model
% Set the "cheat" variable to a value greater than 1
% in order to make the manual control problem easier
%
cheat=6;               % cheat variable
g=9.81/cheat;          % acceleration of gravity (m/s^2)
L=1;lbeam=L*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)
f=5;                   % filter pole
a2=3*g/(4*L);
nump=[-a2/g 0 0];denp=[1 0 -a2];sysp=tf(nump,denp);
[ap,bp,cp,dp]=ssdata(c2d(sysp,dt,'zoh'));
% Zero velocity initialization
icca=inv([cp;cp*ap]);iccb=icca*([dp;cp*bp+dp]); 
xp=icca*[theta;theta]-iccb*xcart;xss=icca*[thmax;thmax];
%
% Initialization - Controller
%
mode = input('Type 1 for manual control, 2 for automatic control:\n');
switch mode
    case 1;gmanual=xmax;    % gain of manual controller
    case 2;invpendcinit;    % 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);
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',[20 300 970 393]);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=[-L,-L];yline1=[0,htrack];
pline1=plot(xline1,yline1,'black');
xline2=[L,L];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');
%
% 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;
    %
    % Controller
    % Control signal:   xref, reference cart position (m)
    % Useful variables: xcart, cart position (m)
    %                   theta, beam angle (rad)
    %                   t, time (s)
    %
    switch mode
        case 1                      % manual control
            scale=get(0,'MonitorPosition');
            cursor=get(0,'PointerLocation');
            xval=2*(cursor(1)-1)/(scale(1,3)-1)-1;
            xcom=gmanual*xval;
        case 2;invpendc;            % automatic control
    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
    %
    % 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;
    %
end
hold off