juggler.m

模糊控制工具箱,很好用的,有相应的说明文件,希望对大家有用!

function [o1,o2,o3] = juggler(x_prev, y_prev, v_prev, delta_t, action)
%JUGGLER Ball juggling system.
%   JUGGLER opens a window for the animation of the ball juggling
%   system. The small triangle indicates the desired ball position.
%   You can choose either human or fuzzy control to control this
%   system. If it is controlled by human, click the steering arrow
%   at the upper right corner to change the tilted angle of the
%   hitting board. If it is controlled by the fuzzy controller, use
%   the mouse to change the location of the triangle. (If fuzzy 
%   controller is selected, the position of the small triangle will
%   also be updated randomly every 200 steps)
%
%   File: juggler.m
%
%   See also FUZDEMOS.

%   Roger Jang, 3-9-94, 10-20-94, 12-23-94. Kelly Liu, 11-20-97
%   Copyright 1994-2004 The MathWorks, Inc.
%   $Revision: 1.17.2.2 $  $Date: 2004/04/10 23:15:21 $

if nargin == 0,
    action = 'initialize';
end

global JugFigH JugFigTitle JugAxisH
global JugAnimRunning JugAnimStepping JugAnimPause JugAnimClose
global JugUpdateBoard JugCount
global JugBallRadius xMin xMax ProjAngle theta
global DesiredPos SamplingTime xNextHit g
global JugFisMat

if strcmp(action, 'next_pos'), % Returns [x, y, v] at the next point
    o1 = x_prev + real(v_prev)*delta_t;
    o2 = y_prev + imag(v_prev)*delta_t - 0.5*g*delta_t^2;
    o3 = real(v_prev) + j*(imag(v_prev) - g*delta_t);
elseif strcmp(action, 'single_loop'),
    % ====== get animation objects
   if ~ishandle(JugFigH)
      return;
   end
    ud = get(JugFigH, 'userdata');
    ballH = ud(3, 1);
    plateH = ud(3, 2);
    refH = ud(3, 3);
    countH = ud(1, 6);
    ball = get(ballH, 'userdata');
    controllerH = ud(2, 1);
    which_controller = get(controllerH, 'value');
    plate = get(plateH, 'userdata');
    ref = get(refH, 'userdata');
    % ====== get state variables
    x_prev = ud(3, 8);
    y_prev = ud(3, 9);
    v_prev = ud(3, 10);
    % ====== Change the desired position randomly if it's fuzzy 
    % this is good for self-running demo
    autoupdate = 1;
    if rem(JugCount, 200) == 0 & which_controller == 2 & autoupdate,
        bound = [xMin+1.5*JugBallRadius xMax-1.5*JugBallRadius];
        DesiredPos = rand*(bound(2)-bound(1))+bound(1);
        new_ref = ref + DesiredPos;
        set(refH, 'xdata', real(new_ref), 'ydata', imag(new_ref));
    end
    % ====== find next position
    [x, y, v] = eval([mfilename, ...
        '(x_prev, y_prev, v_prev, SamplingTime, ''next_pos'')']);

    % ====== When y is out of range
    if y < JugBallRadius,
        JugUpdateBoard = 1;
        vy_prev = imag(v_prev);
        t_hit = (vy_prev+(vy_prev*vy_prev+2*g*(y_prev-JugBallRadius))^0.5)/g;
        x_hit = x_prev + real(v_prev)*t_hit; 
        % When the ball really hits the plate first
        if xMin+JugBallRadius <= x_hit & x_hit <= xMax-JugBallRadius,
            [x, y, v] = ...
            eval([mfilename, ...
            '(x_prev,y_prev,v_prev,''ground'',''reflect'')']);
        end
    end
    % ====== When x is out of range
    if ~(xMin+JugBallRadius <= x & x <= xMax-JugBallRadius),
        JugUpdateBoard = 1;
        if x < xMin + JugBallRadius,
            bound = xMin + JugBallRadius;
        else
            bound = xMax - JugBallRadius;
        end
        t_hit = abs(bound - x_prev)/real(v_prev);
        y_hit = y_prev + imag(v_prev)*t_hit + 0.5*g*t_hit^2;
        % When the ball really hits the wall first
        if y_hit > JugBallRadius & x < xMin+JugBallRadius,
            [x, y, v] = ...
            eval([mfilename, ...
            '(x_prev,y_prev,v_prev,''left_wall'',''reflect'')']);
        elseif y_hit > JugBallRadius & x > xMin+JugBallRadius,
            [x, y, v] = ...
            eval([mfilename, ...
            '(x_prev,y_prev,v_prev,''right_wall'',''reflect'')']);
        end
    end
    % Update plate position when the ball passes its highest point
    % or when the ball hits X bounds while it's falling
    if imag(v) < 0 & JugUpdateBoard,
        vy = imag(v);
        xNextHit = (vy + (vy*vy+2*g*(y-JugBallRadius))^0.5)/g*real(v)+x;
        xNextHit = min(max(xNextHit, ...
            xMin + JugBallRadius), xMax - JugBallRadius);

        if which_controller == 1,   % human controlller
            set(plateH, 'xdata', xNextHit + real(plate));
        else        % fuzzy controller
            theta = ...
            eval([mfilename, '(x, y, v, [], ''fuzzy_controller'')']);
            theta = min(max(theta, -pi/4), pi/4);
            new_plate = plate*exp(j*theta);
            set(plateH, 'ydata', imag(new_plate), ...
                'xdata', xNextHit + real(new_plate));
        end
        JugUpdateBoard = 0;
    end
    % ====== count operation
    set(countH, 'string', int2str(JugCount));
    JugCount = JugCount+1;
    % ====== update ball 
    set(ballH, 'xdata', x + real(ball), 'ydata', y + imag(ball));
    drawnow;
    % ====== put current state variables back into userdata
    ud(3, 8) = x;
    ud(3, 9) = y;
    ud(3, 10) = v;
    if ishandle(JugFigH)
     set(JugFigH, 'userdata', ud);
    end
elseif strcmp(action, 'reflect')
    subaction = delta_t;
    if strcmp(subaction, 'ground'),
        % The ball did hit the plate first
        ProjAngle = ProjAngle + 2*theta;
        % ====== change project angle if it downward.
        if ProjAngle < 0,
            ProjAngle = -ProjAngle;
            fprintf('Bouncing from the ground, project angle = %d\n', ...
                ProjAngle*180/pi);
        elseif ProjAngle > pi,  
            ProjAngle = 2*pi - ProjAngle;
            fprintf('Bouncing from the ground, project angle = %d\n', ...
                ProjAngle*180/pi);
        end

        vy_prev = imag(v_prev);
        t_hit = (vy_prev + (vy_prev*vy_prev+2*g*(y_prev-JugBallRadius))^0.5)/g;
        [x_hit, y_hit, v_in] = ...
        eval([mfilename, '(x_prev, y_prev, v_prev, t_hit, ''next_pos'')']);
        v_out = abs(v_in)*exp(j*ProjAngle);
        [o1, o2, o3] = ...
        eval([mfilename, '(x_hit, y_hit, v_out, SamplingTime-t_hit, ''next_pos'')']);
        % ====== The ball hits the wall also
        if o1 < xMin + JugBallRadius,
            bound = xMin + JugBallRadius;
            [o1, o2, o3] = ...
            eval([mfilename, '(2*bound-x_hit,JugBallRadius, -conj(v_out), SamplingTime-t_hit, ''next_pos'')']);
            ProjAngle = pi - ProjAngle;
        elseif o1 > xMax - JugBallRadius,
            bound = xMax - JugBallRadius;
            [o1, o2, o3] = ...
            eval([mfilename, '(2*bound-x_hit,JugBallRadius, -conj(v_out), SamplingTime-t_hit, ''next_pos'')']);
            ProjAngle = pi - ProjAngle;
        end
        % ====== Stay at ground if the ball cannot bounce
        if o2 < JugBallRadius
            o2 = y_prev;
            o3 = v_prev;
        end
    elseif strcmp(subaction, 'left_wall')
        % The ball did hit the left wall first
        bound = xMin + JugBallRadius;
        x_prev = 2*bound - x_prev;
        v_prev = -conj(v_prev);
        [o1, o2, o3] = ...
        eval([mfilename, '(x_prev, y_prev, v_prev, SamplingTime, ''next_pos'')']);
        ProjAngle = pi - ProjAngle;

        % The ball hits the ground also
        if o2 < JugBallRadius,
            [o1, o2, o3] = ...
            eval([mfilename, '(x_prev, y_prev, v_prev, ''ground'', ''reflect'')']);
        end
    elseif strcmp(subaction, 'right_wall')
        % The ball did hit the right wall first
        bound = xMax - JugBallRadius;
        x_prev = 2*bound - x_prev;
        v_prev = -conj(v_prev);
        [o1, o2, o3] = ...
        eval([mfilename, '(x_prev, y_prev, v_prev, SamplingTime, ''next_pos'')']);
        ProjAngle = pi - ProjAngle;

        % The ball hits the ground also
        if o2 < JugBallRadius,
            [o1, o2, o3] = ...
            eval([mfilename, '(x_prev, y_prev, v_prev, ''ground'', ''reflect'')']);
        end
    else
        fprintf('Subaction string = %s\n', subaction);
        error('Unknown subaction string in ''reflect''!');
    end
elseif strcmp(action, 'initialize'),
    JugFigTitle = 'Ball Juggling Animation';
    JugFigH = findobj(0,'Name', JugFigTitle);
    JugCount = 1;
    if isempty(JugFigH)
        eval([mfilename, '(0,0,0,0,''set_all_gui'')']);
        eval([mfilename, '(0,0,0,0,''set_anim_obj'')']);
        eval([mfilename, '(0,0,0,0,''set_mouse_action'')']);
        eval([mfilename, '(0,0,0,0,''load_fis_mat'')']);
%       eval([mfilename, '(0,0,0,0,''single_loop'')']);
        % ====== change to normalized units
        set(findobj(JugFigH,'Units','pixels'), 'Units','normal');
        % ====== make all UI interruptable
        set(findobj(JugFigH,'Interrupt','off'), 'Interrupt','on');
    else
       % set(gcf, 'color', get(gcf, 'color'));
	refresh(JugFigH);
    end
    eval([mfilename, '(0,0,0,0,''set_constant'')']);
    eval([mfilename, '(0,0,0,0,''set_init_cond'')']);
    ud = get(JugFigH, 'userdata');


%   set(ud(1, 1), 'visible', 'off');
%   set(ud(1, 4:5), 'visible', 'off');
%   set(ud(1, 2:3), 'visible', 'on');
    set(JugFigH, 'HandleVisibility', 'callback');
elseif strcmp(action, 'set_constant'),
    JugAnimRunning = 0;
    JugAnimStepping = 0;
    JugAnimPause = 0;
    JugAnimClose = 0;
    JugUpdateBoard = 1;
    g = 9.8;
    SamplingTime = 0.02;
%elseif strcmp(action, 'set_standard_gui'), % standard UI's
elseif strcmp(action, 'set_all_gui'),
    % ====== standard UI's
    % ====== % No figure, initialize everything
    ui_row_n = 2;   % No. of UI rows
    % ###### default UI settings for SIMUINK ######
    JugFigH = figure( ...
        'Name', JugFigTitle, ...
        'CloseRequestFcn', 'juggler(0, 0, 0, 0, ''stop_anim''); closereq', ...                             
        'NumberTitle', 'off', ...
        'DockControls', 'off');
    % V4 default color
    colordef(JugFigH, 'black');
    % Black background
    set(JugFigH, 'color', [0 0 0]);
    set(0, 'Currentfigure', JugFigH);
    figPos = get(JugFigH, 'position');
    % ====== proportion of UI frame and axes
    ui_area = 0.2;
    axis_area = 1-ui_area;
    % ====== animation area 
    axisPos = [0 figPos(4)*ui_area figPos(3) figPos(4)*axis_area];
    k =  1.0;
    x = axisPos(1); y = axisPos(2); w = axisPos(3); h = axisPos(4);
    axisPos = [x+w*(1-k)/2 y+h*(1-k)/2 w*k, h*k];
    % weird thing: if you don't use normalized unit for
    % axes, patch for ground doesn't appear
    axisPos = axisPos./[figPos(3) figPos(4) figPos(3) figPos(4)];
    JugAxisH = axes('unit', 'normal', 'pos', axisPos-[0 .0 0 0]);
    set(JugAxisH, 'visible', 'off');
    % ====== background frame
    coverPos = [0 0 figPos(3) figPos(4)*ui_area];
    [frameH, framePos] = uiarray(coverPos, 1, 1, 0);
    % ====== rows for UI controls
    spacing = 5;
    [H, Pos] = uiarray(framePos, ui_row_n, 1, spacing);
    % ====== split lower-most rows into 2 uneven regions
    delete(H(2));
    [tmpH, tmpPos] = uiarray(Pos(2,:), 1, 6, 0, spacing);
    % lower left frame
    delete(tmpH(2:4));
    lPos = tmpPos(1, :);
    lPos(3) = 4*lPos(3)+3*spacing;
    set(tmpH(1), 'pos', lPos);
    % lower right frame
    delete(tmpH(6));
    rPos = tmpPos(5, :);
    rPos(3) = 2*rPos(3)+spacing;
    set(tmpH(5), 'pos', rPos);
    % ====== lower-right UI's (same for all SL animation)
    cb1 = [mfilename '(0,0,0,0,''info'')'];
    cb2 = [mfilename '(0,0,0,0,''close'')'];
    [lrH, lrPos] = uiarray(rPos, 1, 2, spacing, spacing, ...
        str2mat('push', 'push'), ...
        str2mat(cb1, cb2), ...
        str2mat('Help', 'Close'));
    infoH = lrH(1); set(infoH, 'tag', 'info');
    closeH = lrH(2); set(closeH, 'tag', 'close');
    % ====== lower-left UI's (same for all SL animation)
    cb1 = '';
    cb2 = [mfilename '(0,0,0,0,''set_init_cond'');', ...
           mfilename '(0,0,0,0,''main_loop'');'];
    cb3 = '';
    cb4 = '';
    [llH, llPos] = uiarray(lPos, 1, 4, spacing, spacing, ...
        str2mat('text', 'push', 'text', 'text'), ...
        str2mat(cb1, cb2, cb3, cb4), ...
        str2mat('t = 0', 'Start Animation ...','',''));
    countH = llH(1); set(countH, 'tag', 'count');
    % ====== extend the width of start button
    delete(llH(3:4));
    startH = llH(2); set(startH, 'tag', 'start');
    startPos = llPos(2,:);
    startPos(3) = 3*startPos(3)+2*spacing;
    set(startH, 'pos', startPos);
    % ====== create stop and pause (under start)
    cb1 = [mfilename '(0,0,0,0,''stop_anim'')'];
    cb2 = [mfilename '(0,0,0,0,''pause_anim'')'];
    cb3 = '';
    [h, pos] = uiarray(startPos, 1, 3, 0,spacing,'push', ...
        str2mat(cb1, cb2, cb3), ...
        str2mat('Stop', 'Pause ...', ''));
    set(h, 'visible', 'off');
    stopH = h(1); set(stopH, 'tag', 'stop');
    pauseH = h(2); set(pauseH, 'tag', 'pause');
    % ====== extend the width of pause button
    delete(h(3));
    pausePos = pos(2, :);
    pausePos(3) = 2*pausePos(3)+spacing;
    set(pauseH, 'pos', pausePos);
    % ===== create continue and step (under pause)
    cb1 = [mfilename '(0,0,0,0,''continue_anim'')'];
    cb2 = [mfilename '(0,0,0,0,''step_anim'')'];
    [h, pos] = uiarray(pausePos, 1, 2, 0, spacing, ...
        'push', ...
        str2mat(cb1, cb2), ...
        str2mat('Continue', 'Step'));
    set(h, 'visible', 'off');
    contH = h(1); set(contH, 'tag', 'continue');
    stepH = h(2); set(stepH, 'tag', 'step');
    %===== put UI handles into current figure's user data 
    ud = [startH stopH pauseH contH stepH countH -1 -1 -1 -1];
    set(JugFigH, 'userdata', ud);

%elseif strcmp(action, 'set_extra_gui'),    % extra UI's
    % ====== extra UI
    % ====== The upper UI controls (Specific to each animation)
    cb1 = [mfilename '(0,0,0,0,''show_trail'')'];
    cb2 = [mfilename '(0,0,0,0,''clear_trail'')'];
    cb3 = '';
    cb4 = [mfilename '(0,0,0,0,''set_mouse_action'')'];

    string1 = 'Show Trails';
    string2 = 'Clear Trails';
    string3 = 'Controller:';
    string4 = 'Human|Fuzzy';