No BSD License  

Highlights from
CaToKi V2. System Identification ToolKit in Open loop SISO.

image thumbnail

CaToKi V2. System Identification ToolKit in Open loop SISO.

by

 

04 Apr 2007 (Updated )

Is an easy tool for system idenfication of real procces.

ballbeam(action)
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

Contact us