ballbeam.m

MATLAB 球形摆控制源程序,实用广

        '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