%
% File: invpend.m
% Inverted pendulum simulation with visualization
% and joystick input.
% Created by Marc Bodson. Last revised: 01/18/03.
%
% 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;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)
%theta=5*pi/180;      % use to set an initial beam angle of 5 deg
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];
% 
% 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=[-lcg,-lcg];yline1=[0,htrack];
pline1=plot(xline1,yline1,'black','EraseMode','background');
xline2=[lcg,lcg];yline2=[0,htrack];
pline2=plot(xline2,yline2,'black','EraseMode','background');
xdcart=[wcart/2;wcart/2;-wcart/2;-wcart/2];
ydcart=[0;hcart;hcart;0];
pcart=fill(xdcart,ydcart+htrack+vgap,'blue','EraseMode','background'); 
xdbeam=[wbeam/2;wbeam/2;-wbeam/2;-wbeam/2];
ydbeam=[0;lbeam;lbeam;0];
pbeam=fill(xdbeam,ydbeam+htrack+vgap+vgap+hcart,'red','EraseMode','background'); 
%
% 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;xcom=gmanual*jstick; % manual control
   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