animtbu.m

交流 模糊控制 交流 模糊控制

      TbuSteerCenter = 20 + j*5;
      circle = radius*[exp(j*linspace(0, 2*pi)) 1]+TbuSteerCenter;
      circleH = line(real(circle), imag(circle), ...
         'color', 'red', 'linewidth', 3, 'erase', 'xor');
      
      stick = radius* ...
         [[0 j] nan ...
            [exp(-j*pi/4) 0 exp(-j*pi*3/4)]*0.2+j nan ...
            [0 j]*exp(j*2*pi/3) nan [0 j]*exp(j*4*pi/3)];
      stickH = line(real(stick+TbuSteerCenter), imag(stick+TbuSteerCenter), ...
         'color', 'red', 'linewidth', 2, 'erase', 'xor');
      set(stickH, 'userdata', stick);
      
      delta = ([0 1 -1 0] + j*[0 sqrt(3) sqrt(3) 0])*0.6;
      l_delta = (delta + j*1.1*radius)*exp(j*pi/4) + TbuSteerCenter;
      r_delta = (delta + j*1.1*radius)*exp(-j*pi/4) + TbuSteerCenter;
      delta1H = patch(real(l_delta), imag(l_delta), 'g');
      delta2H = patch(real(r_delta), imag(r_delta), 'g');
      set(delta1H, 'erase', 'xor');
      set(delta2H, 'erase', 'xor');
      % ====== axis settings
      max_x = 25; max_y = 30;
      set(AnimTbuAxisH, 'clim', [1 64], ...
         'xlim', [-max_x max_x], ...
         'ylim', [-1 max_y], ...
         'box', 'on');
      axis equal;
      %       set(AnimTbuAxisH, 'visible', 'off');
      % ====== plot back wall and dock
      x = [-max_x-5 -truck_w*0.8 0 truck_w*0.8 max_x+5];
      y = [0 0 nan 0 0];
      line(x, y, 'linewidth', 2, 'color', 'b');
      line([-truck_w -truck_w truck_w truck_w]*0.8, ...
         [1 -0.5 -0.5 1], 'linewidth', 3, 'color', 'black');
      grid on;
      set(AnimTbuFigH, 'color', get(AnimTbuFigH, 'color'));
      % ====== append the handles as third row of userdata
      f_wheelsH = line(0, 0, 'erase', 'xor', 'color', 'm');
      r_wheelsH = line(0, 0, 'erase', 'xor', 'color', 'c');
      tmp = [truckH f_wheelsH r_wheelsH wheelH stickH ...
            circleH delta1H delta2H -1 -1];
      set(AnimTbuFigH, 'userdata', [get(AnimTbuFigH, 'userdata'); tmp]);
      % ====== set up initial options
      constant_block = [winName, '/Constant'];
      if str2double(get_param(constant_block, 'value')) < 0,
         set(tmp(5:8), 'visible', 'on');
         set(controllerH, 'value', 1);
      else
         set(tmp(5:8), 'visible', 'off');    %fuzzy
         set(controllerH, 'value', 2);
      end
      % ====== change to normalized units
      set(findobj(AnimTbuFigH,'Units','pixels'), 'Units','normalized');
      % ====== mouse actions
      % action when button is first pushed down
      action1 = [mfilename, '([], [], [], [], ''mouse_action1'')'];
      % actions after the mouse is pushed down
      action2 = [mfilename, '([], [], [], [], ''mouse_action2'')'];
      % action when button is released
      action3 = [mfilename, '([], [], [], [], ''mouse_action3'')'];
      
      % temporary storage for the recall in the down_action
      set(AnimTbuAxisH,'UserData',action2);
      
      % set action when the mouse is pushed down
      down_action=[ ...
            'set(AnimTbuFigH,''WindowButtonMotionFcn'',get(AnimTbuAxisH,''UserData''));' ...
            action1];
      set(AnimTbuFigH,'WindowButtonDownFcn',down_action);
      
      % set action when the mouse is released
      up_action=[ ...
            'set(AnimTbuFigH,''WindowButtonMotionFcn'','' '');', action3];
      set(AnimTbuFigH,'WindowButtonUpFcn',up_action);
   end
   % ====== change labels of standard UI controls
   tmp = get(AnimTbuFigH, 'userdata');
   set(tmp(1, 1), 'visible', 'off');
   set(tmp(1, 2:3), 'visible', 'on');
   refresh(AnimTbuFigH);
   sys = [0 0 1 4 0 0];
   set(AnimTbuFigH, 'HandleVisibility', 'on');
elseif nargin == 5, % for callbacks of GUI
   % ###### standard UI controls
   if strcmp(action, 'start_sl'),
      tmp = get(AnimTbuFigH, 'userdata');
      set(tmp(1, 1), 'visible', 'off');
      set(tmp(1, 2:3), 'visible', 'on');
      [winName] = bdroot(gcs);
      set_param(winName, 'SimulationCommand', 'start');
   elseif strcmp(action, 'stop_sl'),
      tmp = get(AnimTbuFigH, 'userdata');
      set(tmp(1, 1), 'visible', 'on');
      set(tmp(1, 2:5), 'visible', 'off');
      [winName] = bdroot(gcs);
      set_param(winName, 'SimulationCommand', 'stop');
   elseif strcmp(action, 'pause_sl'),
      tmp = get(AnimTbuFigH, 'userdata');
      set(tmp(1, 3), 'visible', 'off');
      set(tmp(1, 4:5), 'visible', 'on');
      [winName] = bdroot(gcs);
      set_param(winName, 'SimulationCommand', 'pause');
   elseif strcmp(action, 'step_sl'),
      [winName] = bdroot(gcs);
      set_param(winName, 'SimulationCommand', 'step');
   elseif strcmp(action, 'continue_sl'),
      tmp = get(AnimTbuFigH, 'userdata');
      set(tmp(1, 3), 'visible', 'on');
      set(tmp(1, 4:5), 'visible', 'off');
      [winName] = bdroot(gcs);
      set_param(winName, 'SimulationCommand', 'continue');
   elseif strcmp(action, 'info'),
      helpwin(mfilename);
      %   title = get(AnimTbuFigH, 'Name');
      %   content = ...
      %   ['                                                    '
      %    ' Animation of the truck backer-upper (TBU)          '
      %    ' system. By clicking at the "Controller:"           '
      %    ' pop-up menu, you can choose to control this        '
      %    ' system by yourself or by a fuzzy controller.       '
      %    ' If it is controlled by human, click at the         '
      %    ' steering handle at the lower right corner          '
      %    ' to change the steering angle of the truck.         '
      %    '                                                    '
      %    ' The simulation stops whenever the truck hits       '
      %    ' the back wall. Move the truck by clicking          '
      %    ' inside the truck, or rotate it by  clicking        '
      %    ' at one of its corners.                             '
      %    '                                                    '
      %    ' Animation S-function: animtbu.m                    '
      %    ' SIMULINK file: sltbu.m                             '];
      %   fhelpfun(title, content);
   elseif strcmp(action, 'close'),
      %       [winName, sysName] = get_param;
      %       set_param(winName, 'Simulation running', 'stop');
      delete(AnimTbuFigH);
      
      % ###### additional UI controls
   elseif strcmp(action, 'controller'),
      tmp = get(AnimTbuFigH, 'userdata');
      controllerH = tmp(2, 2);
      value = get(controllerH, 'value');
      [winName] = bdroot(gcs);
      constant_block = [winName, '/Constant'];
      if value == 1,  % human controller
         set_param(constant_block, 'value', '-1');
         set(tmp(3, 5:8), 'visible', 'on');
      else        % fuzzy controller
         set_param(constant_block, 'value', '1');
         set(tmp(3, 5:8), 'visible', 'off');
      end
   elseif strcmp(action, 'show_trail'),
      tmp = get(AnimTbuFigH, 'userdata');
      dispmodeH = tmp(2, 1);
      objectH = tmp(3, 1:4);
      dispmode = get(dispmodeH, 'value');
      if dispmode == 0,   % xor
         set(objectH, 'erasemode', 'xor');
      else
         set(objectH, 'erasemode', 'none');
      end
   elseif strcmp(action, 'clear_trail'),
      refresh(AnimTbuFigH);
      %        set(AnimTbuFigH, 'color', get(AnimTbuFigH, 'color'));
   elseif strcmp(action, 'mouse_action1')
      TbuSlStatus = get_param('sltbu', 'simulationStatus');
      tmp = get(AnimTbuFigH, 'userdata');
      objectH = tmp(3, 1:4);
      curr_info = get(AnimTbuAxisH, 'CurrentPoint');
      TbuCurrPt = curr_info(1,1) + j*curr_info(1,2);
      if strcmp(TbuSlStatus, 'stopped'),
         now_truck = (TbuTruck+truck_l/2)*exp(j*TbuInitCond(3)) + ...
            TbuInitCond(1) + j*TbuInitCond(2);
         if (inside(TbuCurrPt, now_truck))
            AnimTbuTranslate = 1;
            set(AnimTbuFigH,'pointer', 'crosshair');
         elseif (~isempty(find(objectH == gco)))
            AnimTbuRotate = 1;
            set(AnimTbuFigH,'pointer', 'circle');
         end
         TbuOriDispMode = get(objectH(1), 'erasemode');
         set(objectH, 'erasemode', 'xor');
      else
         stickH = tmp(3, 5); 
         stick = get(stickH, 'userdata');
         theta = angle(-j*(TbuCurrPt - TbuSteerCenter));
         theta = min(max(theta, -pi/4), pi/4);
         new_stick = stick*exp(j*theta) + TbuSteerCenter;
         set(stickH, 'xdata', real(new_stick));
         set(stickH, 'ydata', imag(new_stick));
         TbuSteerAngle = theta;  
      end
   elseif strcmp(action, 'mouse_action2')
      if strcmp(TbuSlStatus, 'stopped'),
         prev_pt = TbuCurrPt;
         curr_info = get(AnimTbuAxisH, 'CurrentPoint');
         TbuCurrPt = curr_info(1,1) + j*curr_info(1,2);
         curr_xy = TbuInitCond(1) + j*TbuInitCond(2);
         phi = TbuInitCond(3);
         if AnimTbuRotate,   % rotation
            truck_center = truck_l/2*exp(j*phi) + curr_xy;
            angle1 = angle(prev_pt - truck_center);
            angle2 = angle(TbuCurrPt - truck_center);
            % rotate truck
            rot_angle = angle2 - angle1;
            next_xy = (curr_xy - truck_center)*exp(j*rot_angle) + truck_center;
            next_phi = phi + rot_angle;
            new_state = [real(next_xy) imag(next_xy) next_phi];
            TbuInitCond = new_state;
            eval([mfilename '(0, [], TbuInitCond, 2);']);
         elseif AnimTbuTranslate,    % translation
            displacement = TbuCurrPt - prev_pt;
            next_xy = curr_xy + displacement;
            new_state = [real(next_xy) imag(next_xy) phi];
            TbuInitCond = new_state;
            eval([mfilename '(0, [], TbuInitCond, 2);']);
         end;
      else
         curr_info = get(AnimTbuAxisH, 'CurrentPoint');
         TbuCurrPt = curr_info(1,1) + j*curr_info(1,2);
         theta = angle(-j*(TbuCurrPt - TbuSteerCenter));
         theta = min(max(theta, -pi/4), pi/4);
         TbuSlStatus = get_param('sltbu', 'simulationStatus');
         tmp = get(AnimTbuFigH, 'userdata');
         stickH = tmp(3, 5); 
         stick = get(stickH, 'userdata');
         new_stick = stick*exp(j*theta) + TbuSteerCenter;
         set(stickH, 'xdata', real(new_stick));
         set(stickH, 'ydata', imag(new_stick));
         TbuSteerAngle = theta;  
      end
   elseif strcmp(action, 'mouse_action3')
      if strcmp(TbuSlStatus, 'stopped'),
         tmp = get(AnimTbuFigH, 'userdata');
         objectH = tmp(3, 1:4);
         set(objectH, 'erasemode', TbuOriDispMode);
         set(AnimTbuFigH, 'pointer', 'arrow');
         AnimTbuTranslate = 0;
         AnimTbuRotate = 0;
      else
         curr_info = get(AnimTbuAxisH, 'CurrentPoint');
         TbuCurrPt = curr_info(1,1) + j*curr_info(1,2);
         theta = angle(-j*(TbuCurrPt - TbuSteerCenter));
         theta = min(max(theta, -pi/4), pi/4);
         tmp = get(AnimTbuFigH, 'userdata');
         stickH = tmp(3, 5); 
         stick = get(stickH, 'userdata');
         new_stick = stick*exp(j*theta) + TbuSteerCenter;
         set(stickH, 'xdata', real(new_stick));
         set(stickH, 'ydata', imag(new_stick));
         TbuSteerAngle = theta;  
      end
   else
      fprintf('Unknown action string: %s.\n', action);
   end
end