function [g_msg,g_status]=nma_MAE121_spring_2010_lab4Main(data) %called by GUI to implement the numerical solution for Lab4 UC davis %called by GUI to implement the numerical solution for Lab4 %UC davis, spring 2011 %see lab4.m for the main GUI file which calls this function %by Nasser M. Abbasi g_msg = ''; g_status =1; options = odeset('OutputFcn',@output); %set up the initial conditions IC = [data.vZero; % v(0) data.rZero; % r(0) data.thetaZero; % theta(0) 0; % X(0) 0]; % Y(0) t = [0 data.maxt]; % simulation time if get(data.handles.nonlinearModelBtn,'Value')==1 [t,sol] = ode15s(@rhsNONLINEAR,t,IC',options); % numerically solve the system else [t,sol] = ode15s(@rhsLINEAR,t,IC'); % numerically solve the system end generatePlots(t,sol,data); % plot the solution % DONE that is all. %------------ % Numerical solver, non-linear equations of motion % This solver solves the set of equations of motion for the car % dynamic model using the non-linear version of the equations. % see report for the equations description %------------ function dy=rhsNONLINEAR(t,y) %order is V,r,theta,X,Y theta = y(3); r = y(2); v = y(1); steering = data.deltaZero*sin(2*pi*data.f*t); alphaf = steering-atan ( (v+data.a*r) / data.U ); alphar = atan ( (data.b*r-v) / data.U ); Yf = data.Cf*alphaf; Yr = data.Cr*alphar; dy = zeros(5,1); dy(1) = (Yf/data.m)*cos(steering)+Yr/data.m-data.U * r; %V dy(2) = (data.a*Yf/data.Ic)*cos(steering)-data.b*Yr/data.Ic; %r dy(3) = r; %theta dy(4) = data.U*cos(theta)-v*sin(theta); %X dy(5) = data.U*sin(theta)+v*cos(theta); %Y end %------------ % Numerical solver, linear equations of motion % This solver solves the set of equations of motion for the car % dynamic model using the linear version of the equations. % see report for the equations description %------------ function dy=rhsLINEAR(t,y) %order is V,r,theta,X,Y alphaf=data.deltaZero*sin(2*pi*data.f*t)-... (y(1)+data.a*y(2))/data.U ; alphar=(data.b*y(2)-y(1))/data.U; Yf = data.Cf*alphaf; Yr= data.Cr*alphar; dy=zeros(5,1); dy(1)=Yf/data.m + Yr/data.m - data.U * y(2); %V dy(2)=data.a*Yf/data.Ic - data.b*Yr/data.Ic; %r dy(3)=y(2); %theta dy(4)=data.U*cos(y(3))-y(1)*sin(y(3)); %X dy(5)=data.U*sin(y(3))+y(1)*cos(y(3)); %Y end %----------------------------------- % %----------------------------------- function output(t,x,~) userData = get(data.handles.figure1, 'UserData'); if userData.stop status=true; else status=false end end end %-------------------------------- % This function is called after the solver is completed % to generate the simulation and plots to the GUI %-------------------------------- function generatePlots(t,sol,data) %read the solutions from the sol matrix v=sol(:,1); r=sol(:,2); maxR=max(r); minR=min(r); if abs(maxR-minR)<2*eps maxR=1; minR=-1; end theta=sol(:,3); X=sol(:,4); Y=sol(:,5); MPH=(60*60)/5280; SCREEN_CAPTURE=false; %set to zero for animation capture if SCREEN_CAPTURE frameNumber = 0; actualFrameNumber = 0; theFrame=getframe(gcf); [im,MAP]=rgb2ind(theFrame.cdata,32,'nodither'); end b=data.b; a=data.a; L=data.L; %------- plot global trajectory, reset plots set(0,'CurrentFigure',data.handles.figure1); cla(data.handles.mainAxes,'reset'); cla(data.handles.fixedBodyAxes,'reset'); cla(data.handles.tireForceAxes,'reset'); cla(data.handles.YrAxes,'reset'); cla(data.handles.slipAngleAxes,'reset'); cla(data.handles.slipAngleRAxes,'reset'); cla(data.handles.rVsTimeAxes,'reset'); maxX=max(X); minX=min(X); maxY=max(Y); minY=min(Y); maxX=maxX+0.05*abs(maxX); minX=min(X)-0.05*abs(minX); if minX==maxX minX=-1; maxX=1; end maxY=max(Y)+0.05*abs(maxY); minY=min(Y)-0.05*abs(minY); if minY==maxY minY=-1; maxY=1; end tireWidth=b/8; tireLength=b/2; leftTireX=[-b -b-tireLength -b-tireLength -b]; leftTireY=[tireWidth/2 tireWidth/2 -tireWidth/2 -tireWidth/2]; frontLeftTireX=[-tireLength/2 -tireLength/2 tireLength/2 tireLength/2]; frontLeftTireY=[-tireWidth/2 tireWidth/2 tireWidth/2 -tireWidth/2]; C=.5*ones(1,length(leftTireY)); carWidth=a/2; bodyFrameX=[-.8*b -.8*b .5*a .65*a .5*a]; bodyFrameY=[-carWidth/2 carWidth/2 carWidth/2 0 -carWidth/2]; set(data.handles.figure1, 'CurrentAxes',data.handles.mainAxes); xlim([minX maxX]); ylim([minY maxY]); grid; %graphics layout uLine=[0 0.25*a 0.1*a 0.25*a 0.1*a; ... 0 0 0.1*a 0 -0.1*a]; vLinePositive=[0 0 -0.1*a 0 0.1*a; ... 0 0.5*a 0.35*a 0.5*a 0.35*a]; vLineNegative=[0 0 -0.1*a 0 0.1*a; ... 0 -0.5*a -0.35*a -0.5*a -0.35*a]; [Yf,Yr,maxYY,minYY,alphaF,alphaR]=getTireForces(t,v,r,data); max_Yf=max(abs(Yf)); max_Yr=max(abs(Yr)); %normalized_Yf=Yf/max_Yf; %normalized_Yr=Yr/max_Yr; set(data.handles.figure1, 'CurrentAxes',data.handles.tireForceAxes); xlim([0 t(end)]); ylim([minYY maxYY]); grid; set(data.handles.figure1, 'CurrentAxes',data.handles.YrAxes); xlim([0 t(end)]); ylim([minYY maxYY]); grid; %update r vs time plot, do this here before start the animation set(data.handles.figure1, 'CurrentAxes',data.handles.rVsTimeAxes); plot(r(1:50:end)*180/pi,'r.-','LineWidth',1); %plot(r*180/pi,'r.-','LineWidth',1); title('r(t) vs time'); xlabel('time (sec)'); ylabel('yaw rate (deg/sec)'); hold on; grid on; %set(gca,'XTickLabel',[0 20 40 60 80 100]); %xlim([0 t(end)]); %ylim([minR-(abs(0.1*minR)) maxR+abs(0.1*maxR)]); set(gca,'FontSize',7); %set up various coordinates for the arrows to draw on the car as simulation %is running. Then use transformation to draw them %negative is down, color red negativeForceArrowOnBackWheel=... [-b-tireLength/2 -b-tireLength/2 -b-tireLength/2-a/10 ... -b-tireLength/2 -b-tireLength/2+a/10; -tireWidth/2 -tireWidth/2-0.3*a -tireWidth/2-0.2*a ... -tireWidth/2-0.3*a -tireWidth/2-0.2*a]; positiveForceArrowOnBackWheel=... [-b-tireLength/2-a/10 -b-tireLength/2 -b-tireLength/2+a/10 ... -b-tireLength/2 -b-tireLength/2; -tireWidth/2-0.2*a -tireWidth/2 -tireWidth/2-0.2*a ... -tireWidth/2 -tireWidth/2-0.4*a]; %front force arrows negativeForceArrowOnFrontWheel=... [0 0 -a/10 ... 0 a/10; -tireWidth/2 -tireWidth/2-0.3*a -tireWidth/2-0.2*a ... -tireWidth/2-0.3*a -tireWidth/2-0.2*a]; positiveForceArrowOnFrontWheel=... [-a/10 0 a/10 ... 0 0; -tireWidth/2-0.2*a -tireWidth/2 -tireWidth/2-0.2*a ... -tireWidth/2 -tireWidth/2-0.4*a]; %Now process the solution we allready have, and update plots and %gui for each time step nTimeSteps= length(t); if nTimeSteps<1000 del=10; elseif nTimeSteps>=1000 && nTimeSteps<10000 del=20; elseif nTimeSteps>=10000 && nTimeSteps<100000 del=40; else del=60; end for i=2:del:length(t) %plot global trajectory set(data.handles.figure1, 'CurrentAxes',data.handles.mainAxes); hold on; line(X(i-1:i),Y(i-1:i),'LineWidth',1.5,'LineStyle','-',... 'color','red'); title('global car motion view'); %ylabel('Y'); xlabel('X'); set(gca,'FontSize',7); set(data.handles.figure1, 'CurrentAxes',... data.handles.fixedBodyAxes); cla; line([-b*cos(theta(i))-carWidth/2*sin(theta(i)), ... a*cos(theta(i))-carWidth/2*sin(theta(i))],... [-b*sin(theta(i))+carWidth/2*cos(theta(i)),... a*sin(theta(i))+carWidth/2*cos(theta(i))],... 'LineWidth',1.5,'LineStyle','-',... 'color','red'); line([-b*cos(theta(i))+carWidth/2*sin(theta(i)), ... a*cos(theta(i))+carWidth/2*sin(theta(i))],... [-b*sin(theta(i))-carWidth/2*cos(theta(i)),... a*sin(theta(i))-carWidth/2*cos(theta(i))],... 'LineWidth',1.5,'LineStyle','-',... 'color','red'); line([-L L ],... [0 0 ],... 'LineWidth',0.5,'LineStyle','-.',... 'color','black'); line([0 0],... [-L L],... 'LineWidth',0.5,'LineStyle','-.',... 'color','black'); %Now do back wheel %rotate back wheel at orgin A = [cos(theta(i)) -sin(theta(i));sin(theta(i)) cos(theta(i))]; rotatedCoordinates=A*[ leftTireX;leftTireY]; %move from 0,0 to left back rotatedCoordinates(2,:)=rotatedCoordinates(2,:)+... carWidth/2*cos(theta(i)); rotatedCoordinates(1,:)=rotatedCoordinates(1,:)-... carWidth/2*sin(theta(i)); patch(rotatedCoordinates(1,:),rotatedCoordinates(2,:),C); %move from 0,0 to right back rotatedCoordinates(2,:)=rotatedCoordinates(2,:)-carWidth*cos(theta(i)); rotatedCoordinates(1,:)=rotatedCoordinates(1,:)+carWidth*sin(theta(i)); patch(rotatedCoordinates(1,:),rotatedCoordinates(2,:),C); %add force arrow to left wheel if Yr(i)<0 %CHECK forceArrow=negativeForceArrowOnBackWheel; lineColor='red'; else forceArrow=positiveForceArrowOnBackWheel; lineColor='blue'; end %move for force arrow to right back tire only rotatedCoordinates=A*[ forceArrow(1,:);forceArrow(2,:)]; rotatedCoordinates(2,:)=rotatedCoordinates(2,:)-... carWidth/2*cos(theta(i)); rotatedCoordinates(1,:)=rotatedCoordinates(1,:)+... carWidth/2*sin(theta(i)); line(rotatedCoordinates(1,:),rotatedCoordinates(2,:),... 'color',lineColor); %put text on the end of the arrow of the force amount if Yr(i)<0 %CHECK text(rotatedCoordinates(1,2)+0.1*a*cos(theta(i)),... rotatedCoordinates(2,2)-0.1*a*cos(theta(i)),... sprintf('%3.3f',abs(Yr(i))),'interpreter','latex'); else text(rotatedCoordinates(1,5)+0.1*a*cos(theta(i)),... rotatedCoordinates(2,5)-0.1*a*cos(theta(i)),... sprintf('%3.3f',abs(Yr(i))),'interpreter','latex'); end %do front wheels steeringAngle= data.deltaZero*sin(2*pi*t(i)*data.f); B = [cos(theta(i)+ steeringAngle) -sin(theta(i)+ steeringAngle);... sin(theta(i)+ steeringAngle) cos(theta(i)+ steeringAngle)]; rotatedCoordinates=B*[ frontLeftTireX; frontLeftTireY]; %move the tire to the right front rotatedCoordinates(1,:)=rotatedCoordinates(1,:)+a*cos(theta(i)); rotatedCoordinates(2,:)=rotatedCoordinates(2,:)+a*sin(theta(i)); %move the tire to the left front rotatedCoordinates(2,:)=rotatedCoordinates(2,:)+... carWidth/2*cos(theta(i)); rotatedCoordinates(1,:)=rotatedCoordinates(1,:)-... carWidth/2*sin(theta(i)); patch(rotatedCoordinates(1,:),rotatedCoordinates(2,:),C); %right front rotatedCoordinates(2,:)=rotatedCoordinates(2,:)-carWidth*cos(theta(i)); rotatedCoordinates(1,:)=rotatedCoordinates(1,:)+carWidth*sin(theta(i)); patch(rotatedCoordinates(1,:),rotatedCoordinates(2,:),C); %add force arrow to right front wheel if Yf(i)<0 %%CHECK forceArrow=negativeForceArrowOnFrontWheel; lineColor='red'; else forceArrow=positiveForceArrowOnFrontWheel; lineColor='blue'; end %move for force arrow to right front tire only rotatedCoordinates=B*[ forceArrow(1,:);forceArrow(2,:)]; rotatedCoordinates(2,:)=rotatedCoordinates(2,:)+a*sin(theta(i)); rotatedCoordinates(1,:)=rotatedCoordinates(1,:)+a*cos(theta(i)); rotatedCoordinates(2,:)=rotatedCoordinates(2,:)-carWidth/2*cos(theta(i)); rotatedCoordinates(1,:)=rotatedCoordinates(1,:)+carWidth/2*sin(theta(i)); line(rotatedCoordinates(1,:),rotatedCoordinates(2,:),'color',lineColor); %put text on the end of the arrow of force amount if Yf(i)<0 %CHECK text(rotatedCoordinates(1,2)+0.05*a*cos(theta(i)),... rotatedCoordinates(2,2)-0.05*a*cos(theta(i)),... sprintf('%3.3f',abs(Yf(i))),'interpreter','latex'); else text(rotatedCoordinates(1,5)+0.05*a*cos(theta(i)),... rotatedCoordinates(2,5)-0.05*a*cos(theta(i)),... sprintf('%3.3f',abs(Yf(i))),'interpreter','latex'); end %do centered frame U , V arrow lines rotatedCoordinates=A*[ bodyFrameX; bodyFrameY]; patch(rotatedCoordinates(1,:),rotatedCoordinates(2,:),... 0.5*ones(1,length( bodyFrameX))); rotatedCoordinates=A*uLine; line(rotatedCoordinates(1,:),rotatedCoordinates(2,:),'color','black'); %add value of U on top of arrow text(rotatedCoordinates(1,2)+0.1*a*cos(theta(i)),... rotatedCoordinates(2,2)+0.1*a*sin(theta(i)),... sprintf('%3.1f',data.U*MPH),'interpreter','latex'); %check if velocity (lateral) on the car is positive or negative if v(i)<=0 rotatedCoordinates=A*vLineNegative; lineColor='red'; delX=0.1*a*sin(theta(i)); delY=-0.1*a*cos(theta(i)); else rotatedCoordinates=A*vLinePositive; lineColor='black'; delX=-0.1*a*sin(theta(i)); delY=0.1*a*cos(theta(i)); end line(rotatedCoordinates(1,:),rotatedCoordinates(2,:),'color',lineColor); %add value of V on top of arrow text(rotatedCoordinates(1,2)+delX,rotatedCoordinates(2,2)+delY,... sprintf('%3.2f',abs(v(i))*MPH),'interpreter','latex'); title('body fixed coordinates view'); if get(data.handles.overSteerBtn,'Value')==1 if abs(alphaF(i))*180/pi > 8 if abs(alphaF(i))*180/pi > 15 text(-6,6,... 'CRITICAL! front slip angle out of control, UNSTABLE!',... 'FontSize',10,'Color','red') else text(-6,6,... 'WARNING! front tire slip angle too large','FontSize',... 10,'Color','red') end else if abs(alphaR(i))*180/pi > 8 if abs(alphaF(i))*180/pi > 15 text(-6,6,... 'CRITICAL! rear slip angle out of control, UNSTABLE!',... 'FontSize',10,'Color','red') else text(-6,6,... 'WARNING! back tire slip angle too large',... 'FontSize',10,'Color','red') end end end end %add resultant speed alpha=atan(v(i)/data.U); valueOfSpeed=sqrt(v(i)^2+data.U^2); %lengthOfSpeed=1.5*b; %sqrt((0.25*a)^2+(0.5*a)^2); %Xcoord=lengthOfSpeed*cos(alpha); %Ycoord=lengthOfSpeed*sin(alpha); CoordinatesOfArrow=[0 1.5*a 1.4*a 1.5*a 1.4*a;... 0 0 0.1*a 0 -0.1*a]; comAngle=alpha+theta(i); B = [cos(comAngle) -sin(comAngle);... sin(comAngle) cos(comAngle)]; rotatedCoordinates=B*[CoordinatesOfArrow(1,:);CoordinatesOfArrow(2,:)]; line(rotatedCoordinates(1,:),rotatedCoordinates(2,:),... 'color','magenta',... 'LineWidth',1.5,'LineStyle','-'); if rotatedCoordinates(2,2)<=0 delX=0.1*a; delY=-0.1*a; else delX=-0.1*a; delY=0.1*a; end text(rotatedCoordinates(1,2)+delX,rotatedCoordinates(2,2)+delY,... sprintf('%3.2f mph',valueOfSpeed*MPH),'interpreter','latex'); xlim([-data.L data.L]); ylim([-data.L data.L]); axis equal; axis tight; hold off; drawnow; %tire forces plot set(data.handles.figure1, 'CurrentAxes',data.handles.tireForceAxes); hold on; plot(t(i-1:i),Yf(i-1:i),'k-','LineWidth',1); title('Force on front tire vs. time(sec)'); %ylabel('Newton'); set(gca,'FontSize',7); drawnow; set(data.handles.figure1, 'CurrentAxes',data.handles.YrAxes); hold on; plot(t(i-1:i),Yr(i-1:i),'k-','LineWidth',1); title('Force on rear tire vs. time(sec)'); %ylabel('Newton'); set(gca,'FontSize',7); %make slip angle plot set(data.handles.figure1, 'CurrentAxes',data.handles.slipAngleAxes); hold on; plot(alphaF(i-1:i)*180/pi,Yf(i-1:i),'r-','LineWidth',1); title('front tire force vs slip angle'); %xlabel('slip angle in deg'); %ylabel('Yf (lb)'); ylim([-10*data.Cf*pi/180 10*data.Cf*pi/180]); xlim([-10 10]); set(gca,'FontSize',7); %updateSimulationOutput_lab4_eme121(data.handles,t(i),v(i),... % r(i),Yf(i),Yr(i),theta(i),valueOfSpeed,data.Cf,data.Cr,... % alphaF(i),alphaR(i),steeringAngle) ; %drawnow; set(data.handles.figure1, 'CurrentAxes',data.handles.slipAngleRAxes); hold on; plot(alphaR(i-1:i)*180/pi,Yr(i-1:i),'r-','LineWidth',1); title('rear tire force vs slip angle'); %xlabel('slip angle in deg'); %ylabel('Yr (lb)'); ylim([-10*data.Cr*pi/180 10*data.Cr*pi/180]); xlim([-10 10]); set(gca,'FontSize',7); updateSimulationOutput_lab4_eme121(data.handles,t(i),v(i),... r(i),Yf(i),Yr(i),theta(i),valueOfSpeed,data.Cf,data.Cr,... alphaF(i),alphaR(i),... steeringAngle) ; drawnow; if SCREEN_CAPTURE frameNumber = frameNumber+1; if mod(frameNumber,1)==0 theFrame = getframe(gcf); actualFrameNumber = actualFrameNumber +1; im(:,:,1,actualFrameNumber ) = ... rgb2ind(theFrame.cdata,MAP,'nodither'); %im=rgb2ind(theFrame.cdata,MAP,'nodither'); %imwrite(im,MAP,[num2str(actualFrameNumber) '.gif'],'gif'); end end userData = get(data.handles.figure1, 'UserData'); if userData.stop break; end end if SCREEN_CAPTURE imwrite(im,MAP,'demo.gif','DelayTime',0,'LoopCount',inf,... 'WriteMode','overwrite'); end end %-------------------------- % Called by the plotting function above to calculate the tire % forces Yf, Yr from the result of the numerical solution % so we can plot them. %-------------------------- function [Yf,Yr,maxY,minY,alphaF,alphaR]=getTireForces(t,v,r,data) Yf=zeros(1,length(t)); Yr=Yf; alphaR=Yr; alphaF=Yr; for i=1:length(t) alphaF(i)=data.deltaZero*sin(2*pi*t(i)*data.f)-... (v(i)+data.a*r(i))/data.U; Yf(i)=data.Cf*alphaF(i); alphaR(i)=(data.b*r(i)-v(i))/data.U; Yr(i)=data.Cr*alphaR(i); end maxY=max([max(Yf),max(Yr)]); minY=min([min(Yf),min(Yr)]); if abs(minY-maxY)<2*eps minY=-1; maxY=1; end end %-------------------------- % Called by the plotting function above to update the GUI % at each time step with current simulation variables %-------------------------- function updateSimulationOutput_lab4_eme121(... handles,t,v,r,Yf,Yr,theta,valueOfSpeed,Cf,Cr,alphaF,alphaR,... steeringAngle) MPH=(60*60)/5280; FTS=1/MPH; set(handles.timeValueTag,'String',sprintf('%3.3f',t)); set(handles.speedValueTag,'String',sprintf('%3.3f',valueOfSpeed*MPH)); set(handles.yawRateValueTag,'String',sprintf('%3.3f',r*180/pi)); set(handles.YfValueTag,'String',sprintf('%3.3f',Yf)); set(handles.YrValueTag,'String',sprintf('%3.3f',Yr)); set(handles.thetaValueTag,'String',sprintf('%3.3f',mod(theta*180/pi,360))); set(handles.CfValueTag,'String',sprintf('%3.3f',Cf*pi/180)); set(handles.CrValueTag,'String',sprintf('%3.3f',Cr*pi/180)); set(handles.steerAngleValueTag,'String',sprintf('%3.3f',steeringAngle*180/pi)); set(handles.alpha_f_valueTag,'String',sprintf('%3.3f',... sign(alphaF)*mod(abs(alphaF)*180/pi,360))); set(handles.alpha_r_valueTag,'String',sprintf('%3.3f',... sign(alphaR)*mod(abs(alphaR)*180/pi,360))); set(handles.V_valueTag,'String',sprintf('%3.3f',v*MPH)); end