%
% File bbeam.m
% Ball and beam simulation with mouse/touchpad input
% and real-time visualization
% Created by Marc Bodson. Last revised: 5/23/25
%
% 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;   % gravity constants
%
% 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');
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');
%
% 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             % manual control
            scale=get(0,'MonitorPosition');
            cursor=get(0,'PointerLocation');
            xval=2*(cursor(1)-1)/(scale(1,3)-1)-1;
            theta=-gmanual*xval;
        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