%
% File bbeam.m
% Ball and beam simulation with visualization 
% and joystick input.
% Created by Marc Bodson. Last revised: 01/28/02.
%
% Initialization - Ball and beam model
%
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;
%
% Initialization - Controller
%
mode = input('Type 1 for manual control, 2 for automatic control:\n');
switch mode;
   case 1;gmanual=thmax; % gain of manual controller
   case 2;bbeamcinit;    % 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.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',[20 350 940 300]);hold on % position/size of sim. window
xdbeam=[lbeam/2;lbeam/2;-lbeam/2;-lbeam/2];
ydbeam=[-wbeam/2;wbeam/2;wbeam/2;-wbeam/2];
pbeam=fill(xdbeam,ydbeam,'green','EraseMode','background');   
xdball=rball*cos(2*pi*ipb/npb);
ydball=rball*sin(2*pi*ipb/npb)+wbeam/2+rball+vgap;
pball=fill(xdball,ydball,'red','EraseMode','background');
xline1=[-0.375*lbeam;-0.375*lbeam];yline1=[-wbeam/2;wbeam/2];
pline1=plot(xline1,yline1,'black','EraseMode','background');
xline2=[0.375*lbeam;0.375*lbeam];yline2=[-wbeam/2;wbeam/2];
pline2=plot(xline2,yline2,'black','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:   theta, beam angle (rad)
% Useful variables: xball, ball position (m)
%                   vball, ball velocity (m/s)
%                   t, time (s)
%
switch mode;
   case 1;theta=-gmanual*jstick;  % manual control 
   case 2;bbeamc;                 % automatic control
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
%
% 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;
%
end
hold off