function [sys,x0,str,ts] = sfrobota(t,x,q,flag,tanim) if flag == 2 [flag,fig]=figflag('Robot',1); if flag==0 return; end h = get(fig(1),'UserData'); if size(h) ~= [1,16] return; end set(h(1),'Visible','off'); % Playback button Rot=[cos(q(3)),+sin(q(3));... -sin(q(3)),cos(q(3))]; Ori=q(1:2); % Tyres tyre=[-.3,-.1;-.3,.1;.3,.1;.3,-.1;-.3,-.1]; % Back tires set(h(3),'xdata',Ori(1)+[tyre(:,1),tyre(:,2)-.7]*Rot(:,1),... 'ydata',Ori(2)+[tyre(:,1),tyre(:,2)-.7]*Rot(:,2)); set(h(4),'xdata',Ori(1)+[tyre(:,1),tyre(:,2)+.7]*Rot(:,1),... 'ydata',Ori(2)+[tyre(:,1),tyre(:,2)+.7]*Rot(:,2)); % Front tire L=1; v=q(4); w=q(5); if (v==0 & w==0) th2=q(3); else th2=atan2(v*sin(q(3))+L*cos(q(3))*w,v*cos(q(3))-L*sin(q(3))*w); end Rot0=[cos(th2),sin(th2);... -sin(th2),cos(th2)]; set(h(5),'xdata',Ori(1)+[L,0]*Rot(:,1)+.7*[tyre(:,1),tyre(:,2)]*Rot0(:,1),... 'ydata',Ori(2)+[L,0]*Rot(:,2)+.7*[tyre(:,1),tyre(:,2)]*Rot0(:,2)); % Car Frame carframe=[0,0;NaN,NaN;-.5,-1;-.5,1;L+.5,1;L+.5,-1;-.5,-1]; % carframe=[-.5,-1;-.5,1;L+.5,1;L+.5,-1;-.5,-1]; set(h(7),'xdata',carframe(:,1),'ydata',carframe(:,2)); set(h(2),'xdata',Ori(1)+carframe*Rot(:,1),'ydata',Ori(2)+carframe*Rot(:,2)); % Switching diagram % if q(5) < 4 % set(h(8),'xdata',[h(9),h(10+q(5))],'ydata',[h(10),h(13+q(5))]); % else % set (h(8),'xdata',[],'ydata',[]); % end drawnow elseif flag==0 sys=[0 % 0 continuous states 1 % 1 discrete states 6 % 5 outputs 5 % dynamically_sized inputs 0 % 0 roots 1 % no direct feedthrough 1]; % 1 sample time ts=[tanim,0]; x0=[0]; [flag,fig]=figflag('Robot',0); if flag==0 % if figure does not exist create it fig=figure( ... 'Name','Robot', ... 'NumberTitle','off', ... 'Color','k', ... 'Visible','on', ... 'Units','normalized', ... 'Position',[0.25 0.25 0.5 0.48], ... % THIS IS WINDOW SIZE/POSITION 'Backingstore','off'); else delete(fig(2:length(fig))) fig=fig(1); h = get(fig(1),'UserData'); if size(h)==[1,16] return; end clf set(fig, ... 'Name','Robot', ... 'NumberTitle','off', ... 'Visible','on', ... 'Units','normalized', ... ... % 'Position',[.51 .98 .47 .5], ... 'Backingstore','off'); end axes( ... 'Units','normalized', ... 'Position',[0 0 1 1], ... 'AspectRatio',[1 1], ... 'Clipping','off', ... 'XLim',[-9 9],'YLim',[-7 9], ... ...% 'XTick',[],'YTick',[], ... 'Drawmode','fast', ... 'Visible','off', ... % the axes 'Box','on', ... ...% 'dataaspectratiomode','auto',... ...% 'plotboxaspectratiomode','auto',... 'NextPlot','add'); car = line( ... 'color','g', ... 'linestyle','-', ... 'markersize',1, ... 'erase','background', ... 'xdata',[],'ydata',[]); whe1 = patch( ... 'EdgeColor','none', ... 'FaceColor','r', ... 'erase','background', ... 'xdata',[],'ydata',[]); whe2 = patch( ... 'EdgeColor','none', ... 'FaceColor','r', ... 'erase','background', ... 'xdata',[],'ydata',[]); whe3 = patch( ... 'EdgeColor','none', ... 'FaceColor','r', ... 'erase','background', ... 'xdata',[],'ydata',[]); whe4 = patch( ... 'EdgeColor','none', ... 'FaceColor','r', ... 'erase','background', ... 'xdata',[],'ydata',[]); setpoint = line( ... 'color','y', ... 'linestyle','-', ... 'markersize',1, ... 'erase','background', ... 'xdata',[],'ydata',[]); % Axis for the switching diagram sw=0; sw1=[0 0]; sw2=zeros(1,6); % [sw,sw1,sw2]=switchdi([.62 .65 .4 .4]); playbk=uicontrol(fig, ... 'Style','push', ... 'String','Playback', ... 'Unit','normalized', ... 'Pos',[0.01 0.01 0.2 0.06], ... 'Call', ... [ 'if exist(''record'')==1,' ... ' robotmovie(record);' ... 'else,' ... ' disp(''Must run simulation first.''),' ... 'end'] ); set(fig,'UserData',[playbk car whe1 whe2 whe3 whe4 setpoint sw sw1(1:2) sw2(1:6)]) drawnow elseif flag==3 sys=[t;q]; elseif flag==9 [flag,fig]=figflag('Robot',1); h = get(fig(1),'UserData'); set(h(1),'Visible','on'); sys=[]; else sys=[]; end