function ballbeam(action)
% BALLBEAM demonsrates Proportional-Derivative (PD) control as applied to a
% ball and beam demonstration experiment. Execute by calling with no arguments.
%
% P and PD controllers are implemented which manipulates the beam angle in
% response to the position of the ball on the beam. The setpoint is indicated
% by the red marker, and is adjusted using the slider at the bottom of the figure.
% The control gain, Kc, and the Derivative Time constant, Td, are adjusted by
% a sliders located in the right hand control panel.
%
% This demo has been tested on Matlab 5.3 and Matlab 6.0. Some of the user
% interface code was shamelessly ripped out of the standard Matlab demos.
%
% Jeff Kantor
% Version 1.0
% March 24, 2001
if nargin < 1
action = 'initialize';
end
persistent ballradius beamlength beamwidth
persistent xmin xmax umin umax g dt
persistent t x v u done
persistent Kc Td N xsp xlast dterm Ad Bd
persistent ballHndl xball yball
persistent beamHndl xbeam ybeam
persistent markerHndl xmarker ymarker
persistent setpointHndl
persistent runHndl resetHndl stopHndl closeHndl infoHndl
persistent gainHndl gaintxtHndl gainlblHndl
persistent dervHndl dervtxtHndl dervlblHndl
persistent manualHndl modeHndl mode
persistent bbAxes plotAxes plotHndl legHndl tdata xdata xspdata
switch lower(action)
case {'initialize'}
% Basic dimensions.
ballradius = 1;
beamlength = 20;
beamwidth = 0.3;
% System parameters
xmin = 0; % Left limit
xmax = beamlength; % Right limit
umin = -0.25; % Minimum beam angle
umax = 0.25; % Maximum beam angle
g = 0.05; % Gravitational Constant
dt = 0.2; % Time Step
% Set initial conditions for ball and beam position
t = 0; % Initial time
x = beamlength/2; % Initial Ball position
v = 0; % Initial Velocity
u = 0; % Initial beam angle
done = 0; % Done Flag
% Setup controller with initial parameters
mode = 1; % Flag, start in manual mode
% Mode == 1 => Manual
% Mode == 2 => Proportional
% Mode == 3 => PD
Kc = 0.1; % Proportional Gain
Td = 20; % Derivative time constant
N = 10; % Maximum derivative gain
xsp = x; % Initial setpoint
xlast = x; % Last measured value
dterm = 0; % Derivative term
Ad = Td/(Td+N*dt); % Derivative filter constant
Bd = Kc*Td*N/(Td+N*dt); % Derivative gain
% Create figure
figNumber = figure( ...
'Name','Ball & Beam Demo', ...
'NumberTitle','off', ...
'BackingStore','off');
% Create axes to hold ball & beam animation
bbAxes = axes(...
'Units','normalized', ...
'Position',[0.05 0.05 0.75 0.70],...
'Visible','off',...
'NextPlot','add');
% Create axes to hold the plotting window
plotAxes = axes(...
'Units','normalized',...
'Position',[0.10 0.70 0.65 0.25],...
'Visible','off');
% Information for all buttons
labelColor=[0.8 0.8 0.8];
yInitPos=0.90;
xPos=0.85;
btnWid=0.10;
btnHt=0.08;
% Spacing between the button and the next command's label
spacing=0.025;
%====================================
% The CONSOLE frame
frmBorder=0.02;
yPos=0.05-frmBorder;
frmPos=[xPos-frmBorder yPos btnWid+2*frmBorder 0.9+2*frmBorder];
h=uicontrol( ...
'Style','frame', ...
'Units','normalized', ...
'Position',frmPos, ...
'BackgroundColor',[0.5 0.5 0.5]);
%====================================
% The START button
btnNumber=1;
yPos=0.90-(btnNumber-1)*(btnHt+spacing);
runHndl=uicontrol( ...
'Style','pushbutton', ...
'Units','normalized', ...
'Position',[xPos yPos-spacing btnWid btnHt], ...
'String','Run', ...
'Interruptible','on', ...
'Callback','ballbeam(''run'');');
%====================================
% The RESET button
btnNumber=2;
yPos=0.90-(btnNumber-1)*(btnHt+spacing);
resetHndl=uicontrol( ...
'Style','pushbutton', ...
'Units','normalized', ...
'Position',[xPos yPos-spacing btnWid btnHt], ...
'String','Reset', ...
'Interruptible','on', ...
'Callback','ballbeam(''reset'');');
%====================================
% The STOP button
btnNumber=3;
yPos=0.90-(btnNumber-1)*(btnHt+spacing);
stopHndl=uicontrol( ...
'Style','pushbutton', ...
'Units','normalized', ...
'Position',[xPos yPos-spacing btnWid btnHt], ...
'Enable','off', ...
'String','Stop', ...
'Callback','ballbeam(''stop'');');
%====================================
% The MODE popup button
btnNumber=4;
yPos=0.90-(btnNumber-1)*(btnHt+spacing);
textStr='Mode';
popupStr=['Manual';' P ';' PD '];
% Generic button information
btnPos1=[xPos yPos-spacing+btnHt/2 btnWid btnHt/2];
btnPos2=[xPos yPos-spacing btnWid btnHt/2];
popupHndl=uicontrol( ...
'Style','text', ...
'Units','normalized', ...
'Position',btnPos1, ...
'String',textStr);
btnPos=[xPos yPos-spacing btnWid btnHt/2];
modeHndl=uicontrol( ...
'Style','popup', ...
'Units','normalized', ...
'Position',btnPos2, ...
'String',popupStr,...
'Value',mode,...
'Callback','ballbeam(''mode'')');
%====================================
% The first slider -- Kc, the control gain
btnNumber=5;
yPos=0.90-(btnNumber-1)*(btnHt+spacing);
gainHndl = uicontrol( ...
'Style','slider', ...
'Units','normalized', ...
'Position',[xPos yPos-btnHt btnWid btnHt/2], ...
'Min',0, ...
'Max', 0.5, ...
'Value', Kc, ...
'Visible','off',...
'Callback','ballbeam(''control'');');
gainlblHndl = uicontrol( ...
'Style','text', ...
'String','Kc', ...
'Units','normalized', ...
'Visible','off',...
'Position',[xPos yPos btnWid btnHt/2]);
gaintxtHndl = uicontrol( ...
'Style','text', ...
'String',num2str(Kc), ...
'Units','normalized', ...
'Visible','off',...
'Position',[xPos yPos-btnHt/2 btnWid btnHt/2]);
if mode >= 2,
set(gainlblHndl,'Visible','on');
set(gaintxtHndl,'Visible','on');
end
%====================================
% The second slider -- Td, the Derivative Time Constant
btnNumber=6;
yPos=0.90-(btnNumber-1)*(btnHt+spacing);
dervHndl = uicontrol( ...
'Style','slider', ...
'Units','normalized', ...
'Position',[xPos yPos-1.5*btnHt btnWid btnHt/2], ...
'Min',0, ...
'Max',50, ...
'Value',Td,...
'Visible','off',...
'Callback','ballbeam(''control'')');
dervlblHndl = uicontrol( ...
'Style','text', ...
'String','Td', ...
'Units','normalized', ...
'Visible','off',...
'Position',[xPos yPos-btnHt/2 btnWid btnHt/2]);
dervtxtHndl = uicontrol( ...
'Style','text', ...
'String',num2str(Td), ...
'Units','normalized', ...
'Visible','off',...
'Position',[xPos yPos-btnHt btnWid btnHt/2]);
if mode == 3,
set(dervlblHndl,'Visible','on');
set(dervtxtHndl,'Visible','on');
end
%====================================
% The INFO button
infoHndl=uicontrol( ...
'Style','push', ...
'Units','normalized', ...
'Position',[xPos 0.16 btnWid btnHt], ...
'String','Info', ...
'Callback','ballbeam(''info'')');
%====================================
% The CLOSE button
closeHndl=uicontrol( ...
'Style','push', ...
'Units','normalized', ...
'Position',[xPos 0.05 btnWid btnHt], ...
'String','Close', ...
'Callback','close(gcf)');
axes(bbAxes);
% Create Ball, keep handle
a = 2*pi*(0:.05:.95)';
xball = ballradius*sin(a);
yball = ballradius*(1+cos(a));
ballHndl = patch(xball+x,yball,1);
set(ballHndl,'EraseMode','xor');
% Create Beam
xbeam = beamlength*[0 1 1 0]';
ybeam = beamwidth*[0 0 -1 -1]';
beamHndl = patch(xbeam,ybeam,2);
set(beamHndl,'EraseMode','xor');
% Create Fulcrum
xpivot = [0 1 -1]';
ypivot = [0 -5 -5]' - beamwidth;
patch(xpivot,ypivot,3);
% Create Setpoint Marker
xmarker = 3*beamwidth*[0 1 -1]';
ymarker = 3*beamwidth*[0 -1 -1]'-beamwidth;
markerHndl = patch(xmarker + xsp,ymarker,'r');
% Draw
axis equal
axis([-2, beamlength+ballradius+1, -5, 5 + ballradius]);
% Create setpoint slider
setpointHndl = uicontrol(gcf, ...
'Units','normalized',...
'Position',[0.05 0.05 0.75 0.04],...
'Style','slider', ...
'Min',0, ...
'Max', beamlength, ...
'Value', beamlength/2,...
'Callback','ballbeam(''setpoint'')');
% Create manual control slider
manualHndl = uicontrol(gcf, ...
'Units','normalized',...
'Position',[0.78 0.12 0.03 0.50],...
'Style','slider',...
'Min',umin,...
'Max',umax,...
'Visible','off',...
'Value',0,...
'Callback','ballbeam(''manual'')');
if mode == 1,
set(manualHndl,'Visible','on');
end
drawnow;
case {'run'}
done = 0;
set([runHndl,resetHndl,closeHndl,infoHndl],'Enable','off');
set(stopHndl,'Enable','on');
set(plotAxes,'Visible','off');
set(plotHndl,'Visible','off');
set(legHndl,'Visible','off');
while (x > xmin) & (x < xmax) & (~done),
% Update controller using current ball position
% Last value of the control is in u
% Current value of the state is in x
if mode == 1,
u = get(manualHndl,'Value');
elseif mode == 2,
pterm = Kc*(x-xsp); % Position mode
dp = Kc*(x-xlast); % Velocity mode
u = 0.02*pterm + 0.98*(u + dp); % Blended
elseif mode == 3
pterm = Kc*(x-xsp);
dterm = Ad*dterm + Bd*(x-xlast);
u = pterm + dterm;
end
xlast = x;
u = max(min(u,umax),umin);
% Compute next ball position
v = v - g*u*dt;
x = x + v*dt;
t = t + dt;
% Store Data
xdata = [xdata,x];
xspdata = [xspdata,xsp];
tdata = [tdata,t];
% Update Ball and Beam Animation
set(beamHndl, ...
'Xdata',cos(u)*xbeam - sin(u)*ybeam, ...
'Ydata',sin(u)*xbeam + cos(u)*ybeam);
set(ballHndl, ...
'Xdata',cos(u)*(x + xball) - sin(u)*yball, ...
'Ydata',sin(u)*(x + xball) + cos(u)*yball);
set(markerHndl, ...
'Xdata',cos(u)*(xsp + xmarker) - sin(u)*ymarker, ...
'Ydata',sin(u)*(xsp + xmarker) + cos(u)*ymarker);
drawnow;
end
if x < xmin
x = xmin - 1 - ballradius;
set(ballHndl,'Xdata',x + xball,'Ydata',yball-5);
end
if x > xmax
x = xmax + ballradius;
set(ballHndl,'Xdata',x + xball,'Ydata',yball-5);
end
% Update Plot
axes(plotAxes);
plotHndl = plot(tdata,xdata,'b',tdata,xspdata,'r');
axis([min(tdata),max(tdata),xmin,xmax]);
xlabel('Time');
ylabel('Beam Position');
legHndl = legend(['Ball Position';...
'Setpoint '],0);
set(plotAxes,'Visible','on');
set([runHndl,resetHndl,closeHndl,infoHndl],'Enable','on');
set(stopHndl,'Enable','off');
axes(bbAxes);
case{'manual'}
% Set the beam angle manually
u = get(manualHndl,'Value');
set(beamHndl, ...
'Xdata',cos(u)*xbeam - sin(u)*ybeam, ...
'Ydata',sin(u)*xbeam + cos(u)*ybeam);
set(ballHndl, ...
'Xdata',cos(u)*(x + xball) - sin(u)*yball, ...
'Ydata',sin(u)*(x + xball) + cos(u)*yball);
set(markerHndl, ...
'Xdata',cos(u)*(xsp + xmarker) - sin(u)*ymarker, ...
'Ydata',sin(u)*(xsp + xmarker) + cos(u)*ymarker);
case('setpoint')
% Get the setpoint from the setpoint slider, then
% adjust the setpoint marker
xsp = get(setpointHndl,'Value');
set(markerHndl, ...
'Xdata',cos(u)*(xsp + xmarker) - sin(u)*ymarker, ...
'Ydata',sin(u)*(xsp + xmarker) + cos(u)*ymarker);
case{'reset'}
% Reset the ball to the middle of the beam, and level the
% beam.
x = beamlength/2;
v = 0;
u = 0;
t = 0;
xsp = x;
xlast = x;
dterm = 0;
xdata = x;
tdata = t;
xspdata = xsp;
set(setpointHndl,'Value',beamlength/2);
set(beamHndl,'Xdata',xbeam,'Ydata',ybeam);
set(ballHndl,'Xdata',x + xball, 'Ydata',yball);
set(markerHndl,'Xdata',xsp + xmarker,'Ydata',ymarker);
case{'mode'}
mode = get(modeHndl,'Value');
if mode == 1,
% Manual Mode
set(manualHndl,'Visible','on');
set(gainlblHndl,'Visible','off');
set(gaintxtHndl,'Visible','off');
set(gainHndl,'Visible','off');
set(dervlblHndl,'Visible','off');
set(dervtxtHndl,'Visible','off');
set(dervHndl,'Visible','off');
set(manualHndl,'Value',u);
elseif mode == 2,
% Proportional Mode
set(manualHndl,'Visible','off');
set(gainlblHndl,'Visible','on');
set(gaintxtHndl,'Visible','on');
set(gainHndl,'Visible','on');
set(dervlblHndl,'Visible','off');
set(dervtxtHndl,'Visible','off');
set(dervHndl,'Visible','off');
elseif mode == 3,
% PD Mode
set(manualHndl,'Visible','off');
set(gainlblHndl,'Visible','on');
set(gaintxtHndl,'Visible','on');
set(gainHndl,'Visible','on');
set(dervlblHndl,'Visible','on');
set(dervtxtHndl,'Visible','on');
set(dervHndl,'Visible','on');
dterm = 0;
end
case{'control'}
% Get the control parameters from the sliders, do them
% both with the same call because I'm too lazy to write
% separate cases.
Kc = get(gainHndl,'Value');
Td = get(dervHndl,'Value');
set(gaintxtHndl,'String',num2str(Kc));
set(dervtxtHndl,'String',num2str(Td));
Ad = Td/(Td+N*dt);
Bd = Kc*Td*N/(Td+N*dt);
case{'info'}
% Display help file
helpwin(mfilename);
case{'stop'}
% Set stop flag. This is caught in run, and the
% button status's reset there on exit from the main
% loop
done = 1;
otherwise
% Should never get here
disp('Unknown action');
end
