function pitchFUN(action)

%**************************************************************%
%This functions is intended for use with the pitchGUI.m file.  %
%It contains the code necessary to run the GUI's animation     %
%and controls.                                                 %
%                                                              %
%Copyright (C) 1997 by the Regents of the University of        %
%Michigan.                                                     %
%**************************************************************%

%The Callback for the Run Button%
if action==1	
	
      	yo=[0 0 0];

      	A=[-0.313       56.7    0;
           -0.0139      -0.426  0;
            0           56.7    0];
                
      	B=[0.232;
           0.0203;
           0];
              
      	C=[0 0 1];

      	D=[0];
      	

%Get the weighing factor from the editable text field%
	whandle = findobj('Tag','wfactor');
	wx = eval(get(whandle,'String'));
	
      	Q=[0 0 0;
           0 0 0;
           0 0 wx];
           
%Use the LQR command to find the K-matrix%             
      	[K]= lqr (A,B,Q,1);

%Check for reference input%
        Nhandle = findobj('Tag','reference');
        Nbarval = get(Nhandle,'Value');
        if Nbarval == 0
           Nbar = 1;
        elseif Nbarval == 1
           s = size(A,1);
           Z = [zeros([1,s]) 1];
           N = inv([A,B;C,D])*Z';
           Nx = N(1:s);
           Nu = N(1+s);
           Nbar=Nu + K*Nx;
        end     

%Get the value of the step input from the slider%        
        stephandle = findobj('Tag','stepslider');
        stepval=get(stephandle,'Value');
        %stepaxis=stepval;
        t=0:0.1:10;
      	de=stepval*ones(size(t));

%Run the simulation%             
      	[Y,X]=lsim (A-B*K,B*Nbar,C,D,de,t,yo);
      	theta=Y;
 
%Place a zero at the first point%    	
      	change1=theta;
      	lt=length(theta);
      	for n1=1:lt-1
      	  change1(1)=0;
      	  change1(n1+1)=theta(n1);
      	end
      	theta=change1;
      	  
      	deltae=-K*X'+Nbar*de;
      	deltae=deltae';
      	ld=length(deltae);
      	change2=deltae;
      	for n2=1:ld-1
      	  change2(1)=0;
      	  change2(n2+1)=deltae(n2);
      	end 
      	deltae=change2; 
 
%Check if the response is to be plotted separately%       	
      	phandle = findobj('Tag','plotbox');
        plotval=get(phandle,'Value');   
        if plotval == 1
           subplot(2,2,2)
           plot(t,theta)
        else
           subplot(2,2,2) 
           plot(t(1),theta(1), 'EraseMode', 'none')
        end

%Set the axis limits for the step response plot%
        if stepval > 0
           axis([0 10 0 stepval*2])
        elseif stepval < 0
           axis([0 10 stepval*2 0])
        else
           axis([0 10 -0.4 0.4])
        end

        title ('Step Response')
      	ylabel ('Pitch angle (rad)')
      	xlabel ('Time (sec)')
        
        hold on
        
%Animation Program begins%
	l=0.7;
	l2=0.15;
	l3=0.3;
	l4=0.6;
	l5=0.3;
	l6=0.05;
	h=0.03;
	h2=0.2;
	c=2*sqrt(h^2+(l/2)^2);
	c2=2*sqrt(h^2+(l3/2)^2);
	c3=2*sqrt(h^2+(l4/2)^2);
	c4=2*sqrt(h^2+(l5/2)^2);
	c5=sqrt(h2^2+(l6)^2);
	phi=atan(2*h/l);
  	phi2=atan(2*h/l3);
  	phi3=atan(2*h/l4);
  	phi4=atan(2*h/l5);
  	phi5=atan(2*h2/l6);
  	
%Lines%
  	xtopnose=c/2*cos(phi+theta);
	ytopnose=c/2*sin(phi+theta);
	xtoptail=-(c/2)*cos(phi-theta);
	ytoptail=(c/2)*sin(phi-theta);

	xbotnose=-c/2*cos(pi-phi+theta);
	ybotnose=-c/2*sin(pi-phi+theta);
	xbottail=(c2/2)*cos(pi-phi2-theta);
	ybottail=-(c2/2)*sin(pi-phi2-theta);

	xtcaptip=(l/2+l2)*cos(theta);
	ytcaptip=(l/2+l2)*sin(theta);
	xtcapend=c/2*cos(phi+theta);
	ytcapend=c/2*sin(phi+theta);

	xbcaptip=(l/2+l2)*cos(theta);
	ybcaptip=(l/2+l2)*sin(theta);
	xbcapend=-c/2*cos(pi-phi+theta);
	ybcapend=-c/2*sin(pi-phi+theta);

	xendtop=-(c/2)*cos(phi-theta);
	yendtop=(c/2)*sin(phi-theta);
	xendbot=-(l/2)*cos(theta);
	yendbot=-(l/2)*sin(theta);
	
	xdiatop=-(l/2)*cos(theta);
	ydiatop=-(l/2)*sin(theta);
	xdiabot=(c2/2)*cos(pi-phi2-theta);
	ydiabot=-(c2/2)*sin(pi-phi2-theta);

	xwlbot=-(c3/2)*cos(phi3-theta);
	ywlbot=(c3/2)*sin(phi3-theta);
	xwltop=-(c3/2)*cos(phi3-theta)-h2*sin(theta);
	ywltop=(c3/2)*sin(phi3-theta)+h2*cos(theta);

	xwrbot=-(c4/2)*cos(phi4-theta);
	ywrbot=(c4/2)*sin(phi4-theta);
	xwrtop=-(c4/2)*cos(phi4-theta)-c5*sin((0.6+pi)/2-phi5+theta);
	ywrtop=(c4/2)*sin(phi4-theta)+c5*cos((0.6+pi)/2-phi5+theta);

	xwtopr=-(c4/2)*cos(phi4-theta)-c5*sin((0.6+pi)/2-phi5+theta);
	ywtopr=(c4/2)*sin(phi4-theta)+c5*cos((0.6+pi)/2-phi5+theta);
	xwtopl=-(c3/2)*cos(phi3-theta)-h2*sin(theta);
	ywtopl=(c3/2)*sin(phi3-theta)+h2*cos(theta);
	
  	elevxl = -(l/3)*cos(theta)-0.12*cos(deltae);
	elevxr = -(l/3)*cos(theta);
	elevyl = -(l/3)*sin(theta)+0.12*sin(deltae);
	elevyr = -(l/3)*sin(theta);
  	
%Center of gravity represented by a dot
  	radius=0.005;
	arcstep = 18; % angle increment (in degrees)
	j = 0:arcstep:(360-arcstep); % create ball sides using arclengths
	arcx = radius * cos((j+arcstep) * pi/180); % x coordinates of ball sides
	arcy = radius * sin((j+arcstep) * pi/180); % y coordinates of ball sides

%Plotting the first frame of the animation%
	subplot(2,2,4)
	cla
	K = plot([xtopnose(1) xtoptail(1)], [ytopnose(1) ytoptail(1)], 'k',...
	 	'EraseMode', 'xor','LineWidth',[2]);
	hold on
	L = plot([xbotnose(1) xbottail(1)], [ybotnose(1) ybottail(1)],...
		'k','EraseMode','xor','LineWidth',[2]);

	M = plot([xtcaptip(1) xtcapend(1)], [ytcaptip(1) ytcapend(1)],'k',...
		'EraseMode','xor','LineWidth',[2]);
	
	N = plot([xbcaptip(1) xbcapend(1)],[ybcaptip(1) ybcapend(1)],'k',...
		'EraseMode','xor','LineWidth',[2]);

	O = plot([xendtop(1) xendbot(1)],[yendtop(1) yendbot(1)],'k',...
		'EraseMode','xor','LineWidth',[2]);
		
	O2 = plot([elevxl(1) elevxr(1)],[elevyl(1) elevyr(1)],'r',...
		'EraseMode','xor','LineWidth',[3]);
	
	P = plot([xdiatop(1) xdiabot(1)],[ydiatop(1) ydiabot(1)],'k',...
		'EraseMode','xor','LineWidth',[2]);	
	
	Q = plot([xwrtop(1) xwrbot(1)],[ywrtop(1) ywrbot(1)],'k',...
		'EraseMode','xor','LineWidth',[2]);
	
	R = plot([xwltop(1) xwlbot(1)],[ywltop(1) ywlbot(1)],'k',...
		'EraseMode','xor','LineWidth',[2]);
		
	S = plot([xwtopr(1) xwtopl(1)],[ywtopr(1) ywtopl(1)],'k',...
		'EraseMode','xor','LineWidth',[2]);
	
	H = patch(arcx, arcy,'b', 'EraseMode', 'xor');
        
        axis([-0.55 0.55 -0.55 0.55])
        
%Check if the frames should advance manually or automaically%
        mhandle = findobj('Tag','manbox');
        man=get(mhandle,'Value');
        if man == 1
           'Press any key to advance the animation'
            pause
        end   
	
	
	ltime=length(t);      
	axis([-0.55 0.55 -0.55 0.55])
	title ('Animation')

%Run the animation%
	for i = 2:ltime-1,
	
	   if man == 1
	      pause
	   end   
      
      	   set(K,'XData',[xtopnose(i) xtoptail(i)]);
      	   set(K,'YData',[ytopnose(i) ytoptail(i)]);
      	   set(L,'Xdata',[xbotnose(i) xbottail(i)]);
      	   set(L,'Ydata',[ybotnose(i) ybottail(i)]);
      	   set(M,'XData',[xtcaptip(i) xtcapend(i)]);
      	   set(M,'YData',[ytcaptip(i) ytcapend(i)]);
      	   set(N,'XData',[xbcaptip(i) xbcapend(i)]);
      	   set(N,'YData',[ybcaptip(i) ybcapend(i)]);
           set(O,'XData',[xendtop(i) xendbot(i)]);
           set(O,'YData',[yendtop(i) yendbot(i)]);
           set(P,'XData',[xdiatop(i) xdiabot(i)]);
           set(P,'YData',[ydiatop(i) ydiabot(i)]);
           set(Q,'XData',[xwrtop(i) xwrbot(i)]);
           set(Q,'YData',[ywrtop(i) ywrbot(i)]);
      	   set(R,'XData',[xwltop(i) xwlbot(i)]);
      	   set(R,'YData',[ywltop(i) ywlbot(i)]);
      	   set(S,'XData',[xwtopr(i) xwtopl(i)]);
      	   set(S,'YData',[ywtopr(i) ywtopl(i)]);
      	   
      	   set(O2,'XData',[elevxl(i), elevxr(i)]);
      	   set(O2,'YData',[elevyl(i), elevyr(i)]);
      	   drawnow;
      	
      	   if plotval == 0
      	      subplot(2,2,2)
      	      plot([t(i),t(i+1)],[theta(i),theta(i+1)], 'EraseMode','none')
      	   end 
      	  
      	end      

%The callback for the Reset button: Initialze plots% 
elseif action == 2
	
	%Constant values
	l=0.7;
	l2=0.15;
	l3=0.3;
	l4=0.6;
	l5=0.3;
	l6=0.05;
	h=0.03;
	h2=0.2;
	c=2*sqrt(h^2+(l/2)^2);
	c2=2*sqrt(h^2+(l3/2)^2);
	c3=2*sqrt(h^2+(l4/2)^2);
	c4=2*sqrt(h^2+(l5/2)^2);
	c5=sqrt(h2^2+(l6)^2);
	phi=atan(2*h/l);
  	phi2=atan(2*h/l3);
  	phi3=atan(2*h/l4);
  	phi4=atan(2*h/l5);
  	phi5=atan(2*h2/l6);
  	theta=0;
  	deltae=0;
  	%Lines
  	xtopnose=c/2*cos(phi+theta);
	ytopnose=c/2*sin(phi+theta);
	xtoptail=-(c/2)*cos(phi-theta);
	ytoptail=(c/2)*sin(phi-theta);

	xbotnose=-c/2*cos(pi-phi+theta);
	ybotnose=-c/2*sin(pi-phi+theta);
	xbottail=(c2/2)*cos(pi-phi2-theta);
	ybottail=-(c2/2)*sin(pi-phi2-theta);

	xtcaptip=(l/2+l2)*cos(theta);
	ytcaptip=(l/2+l2)*sin(theta);
	xtcapend=c/2*cos(phi+theta);
	ytcapend=c/2*sin(phi+theta);

	xbcaptip=(l/2+l2)*cos(theta);
	ybcaptip=(l/2+l2)*sin(theta);
	xbcapend=-c/2*cos(pi-phi+theta);
	ybcapend=-c/2*sin(pi-phi+theta);

	xendtop=-(c/2)*cos(phi-theta);
	yendtop=(c/2)*sin(phi-theta);
	xendbot=-(l/2)*cos(theta);
	yendbot=-(l/2)*sin(theta);
	
	xdiatop=-(l/2)*cos(theta);
	ydiatop=-(l/2)*sin(theta);
	xdiabot=(c2/2)*cos(pi-phi2-theta);
	ydiabot=-(c2/2)*sin(pi-phi2-theta);

	xwlbot=-(c3/2)*cos(phi3-theta);
	ywlbot=(c3/2)*sin(phi3-theta);
	xwltop=-(c3/2)*cos(phi3-theta)-h2*sin(theta);
	ywltop=(c3/2)*sin(phi3-theta)+h2*cos(theta);

	xwrbot=-(c4/2)*cos(phi4-theta);
	ywrbot=(c4/2)*sin(phi4-theta);
	xwrtop=-(c4/2)*cos(phi4-theta)-c5*sin((0.6+pi)/2-phi5+theta);
	ywrtop=(c4/2)*sin(phi4-theta)+c5*cos((0.6+pi)/2-phi5+theta);

	xwtopr=-(c4/2)*cos(phi4-theta)-c5*sin((0.6+pi)/2-phi5+theta);
	ywtopr=(c4/2)*sin(phi4-theta)+c5*cos((0.6+pi)/2-phi5+theta);
	xwtopl=-(c3/2)*cos(phi3-theta)-h2*sin(theta);
	ywtopl=(c3/2)*sin(phi3-theta)+h2*cos(theta);
	
	elevxl = -(l/3)*cos(theta)-0.12*cos(deltae);
	elevxr = -(l/3)*cos(theta);
	elevyl = -(l/3)*sin(theta)-0.12*sin(deltae);
	elevyr = -(l/3)*sin(theta);
  	
%Center of gravity represented by a dot
  	radius=0.005;
	arcstep = 18; % angle increment (in degrees)
	j = 0:arcstep:(360-arcstep); % create ball sides using arclengths
	arcx = radius * cos((j+arcstep) * pi/180); % x coordinates of ball sides
	arcy = radius * sin((j+arcstep) * pi/180); % y coordinates of ball sides

%Plotting the initial position of the plane%
	subplot(2,2,4)
	cla
	K = plot([xtopnose xtoptail], [ytopnose ytoptail], 'k',...
	 	'EraseMode', 'xor','LineWidth',[2]);
	hold on
	
	L = plot([xbotnose xbottail], [ybotnose ybottail],...
		'k','EraseMode','xor','LineWidth',[2]);

	M = plot([xtcaptip xtcapend], [ytcaptip ytcapend],'k',...
		'EraseMode','xor','LineWidth',[2]);
	
	N = plot([xbcaptip xbcapend],[ybcaptip ybcapend],'k',...
		'EraseMode','xor','LineWidth',[2]);

	O = plot([xendtop xendbot],[yendtop yendbot],'k',...
		'EraseMode','xor','LineWidth',[2]);
		
	O2 = plot([elevxl elevxr],[elevyl elevyr],'r',...
		'EraseMode','xor','LineWidth',[3]);	
	
	P = plot([xdiatop xdiabot],[ydiatop ydiabot],'k',...
		'EraseMode','xor','LineWidth',[2]);	
	
	Q = plot([xwrtop xwrbot],[ywrtop ywrbot],'k',...
		'EraseMode','xor','LineWidth',[2]);
	
	R = plot([xwltop xwlbot],[ywltop ywlbot],'k',...
		'EraseMode','xor','LineWidth',[2]);
		
	S = plot([xwtopr xwtopl],[ywtopr ywtopl],'k',...
		'EraseMode','xor','LineWidth',[2]);
	
	H = patch(arcx, arcy,'b', 'EraseMode', 'xor');
	
	hold off
	axis([-0.55 0.55 -0.55 0.55])
	title ('Animation')

%Clear the step response plot%	
	subplot (2,2,2)
	cla
	axis ([0 10 0 0.25])
	ylabel ('Pitch angle (rad)')
	xlabel ('Time (sec)')
	title ('Step Response')

%Updates the slider read-out with its current value%	
elseif action == 3
    stephandle = findobj('Tag','stepslider');
    stepval=get(stephandle,'Value');
    curhandle = findobj('Tag','curtext');
    set(curhandle,'String',sprintf('%6.4f',stepval)); 
    
end