function out = SecondOrd(arg1,arg2,flag,C,tin,Fin)
% SECONDORD  illustrates second order system responses, including
% step response of a spring-mass-damper model
% and sinusoidal forcing function response.
% SECONDORD(t,F) uses columns of time t and forcing function F(t),
% piecewise linear data instead of analytical forcing functions.  
% In this case the response is evaluated numerically using the 
% Matlab function 0DE45() instead of using an analytic solution.

if nargin==6	% ODEFile functionality
	t=arg1;	y=arg2;		% (flag is not used)
	out=y;
	F=interp1(tin,Fin,t);
	out(1)=y(2);		% out(1)=velocity
	out(2)=C*[F;y];		% out(2)=acceleration
	
elseif (nargin==1 & isstr(arg1))	% GUI callback functionality
	action=arg1;
	p=get(gcbf,'UserData');
	switch (action)
	case 'popup'
		switch p.mode
		case 1					% Step mode
			set(p.stepclump,'Visible','off')
			set(p.fnedit,'String',num2str(p.fn))
			set(p.zetaedit,'String',num2str(p.zeta))
			set(p.Kedit,'String',num2str(p.K))
		case 2					% Homogeneous mode
			set(p.constclump,'Visible','off')
		case 3					% Piecewise General mode
			set(p.genclump,'Visible','off')
		case 4					% Start Harmonic mode
			set(p.startclump,'Visible','off')
		case 5					% Steady Harmonic mode
			set(p.steadyclump,'Visible','off')
		end
		p.mode=get(gcbo,'Value');
		switch p.mode
		case 1					% Step mode
			set(p.stepclump,'Visible','on')
			p.fn=sqrt(p.Ks/p.M)/2/pi;
			p.zeta=p.Bf/2/sqrt(p.M*p.Ks);
			p.K=1/p.Ks;
			p=doit(p);
		case 2					% Homogeneous mode
			set(p.constclump,'Visible','on')
			p=doit(p);
		case 3					% Piecewise General mode
			set(p.genclump,'Visible','on')
			p=doit(p);
		case 4					% Start Harmonic mode
			set(p.startclump,'Visible','on')
			p=doit(p);
		case 5					% Steady Harmonic mode
			set(p.steadyclump,'Visible','on')
			p=doit(p);
		end
	case {'M','Bf','Ks'}
		eval(['val=str2num(get(gcbo,''String''));'])
		if (val>0)
			eval(['p.',action,'=val;'])
			p.fn=sqrt(p.Ks/p.M)/2/pi;
			p.zeta=p.Bf/2/sqrt(p.M*p.Ks);
			p.K=1/p.Ks;
			p=doit(p);
		else
			errordlg({['Parameter ',action,' must be >0']})
		end
	case {'fn','zeta','K'}
		eval(['val=str2num(get(gcbo,''String''));'])
		if (val>0)
			eval(['p.',action,'=val;'])
			p=doit(p);
		else
			errordlg({['Parameter ',action,' must be >0']})
		end		
	case {'Fi','Ff','A','f','y0','yp0'}
	   eval(['p.',action,'=str2num(get(gcbo,''String''));'])
	   p=doit(p);	
	case 'return'	% build output format query window
		choice=questdlg('Select Output Time Basis','Numeric Output', ...
			'Resample','ODE45','ODE45');
		switch (choice)
		case 'ODE45'	% get output already calculated for line using
			t=get(p.yline,'XData');	% ode45 internal time basis
			y=get(p.yline,'YData');
		case 'Resample'	% recalculate output using new sample rate
			T=p.t(end)-p.t(1);
			fs=(length(p.t)-1)/T;
			fstr=inputdlg('Enter Sampling Frequency','fs',1,{num2str(fs)});
			fs=str2num(fstr{1});
			trange=p.t(1):1/fs:p.t(end);
			wn=(p.fn*2*pi);		C=wn^2*[p.K, -1, -2*p.zeta/wn];
			[t,r]=ode45('secondord',trange,[p.y0;p.yp0],[],C,p.t,p.F);
			y=r(:,1);
		end
		assignin('base','tout',t(:))
		assignin('base','yout',y(:))
		disp('Returned tout and yout to workspace')
	case 'Response'
		str=sprintf('%0.15f',gcbo);	% relocate window, or build a new one
		bfh=findobj('tag',str);		% the button handle becomes a unique
		if ishandle(bfh)			% tag to identify the response figure
			figure(bfh)
		else
			bfh=figure('tag',str);
			mph=uimenu('Parent',bfh,'Label','disown');
			uimenu('Parent',mph,'Label','disown', ...
				'Callback',['set(gcbf,''Tag'',''''),' ...
					'ans=[0;0];ans(1)=get(gcbo,''Parent'');' ...
					'ans(2)=gcbo;delete(ans)']);
		end
		fr=real(p.fn*sqrt(1-2*p.zeta^2));
		f=[exp(linspace(log(p.fn/10),log(p.fn*10),201)),p.f,fr];
		M=1./sqrt((1-(f/p.fn).^2).^2+(2*p.zeta*f/p.fn).^2);
		phi=180/pi*atan2(-2*p.zeta*f/p.fn,1-(f/p.fn).^2);
		subplot(2,1,1)
		loglog(f(1:201),M(1:201),'b',f(202),M(202),'ko')
		if (p.zeta<0.707)
			hold on,	loglog(f(203),M(203),'b^'),		hold off
		end
		xlabel('Frequency'),ylabel('Magnitude Ratio M')
		title('Frequency Response')
		set(gca,'XLim',[min(f) max(f)])
		subplot(2,1,2)
		semilogx(f(1:201),phi(1:201),'b',f(202),phi(202),'ok')
		xlabel('Frequency'),ylabel('Phase Shift \phi (\circ)')
		set(gca,'XLim',[min(f) max(f)],'YLim',[-180 0])
	case 'p'
		out=p;
	case 'help'
    	helpwin('aboutsecondord')
	otherwise
		disp(['callback ',action,' unknown'])
	end
	set(gcbf,'UserData',p)

elseif (nargin==0 | (nargin==2 & isreal(arg1) & isreal(arg2)))
	if nargin==0					% easy invocation
		t=[0 0.499 0.5 0.999 1 1.499 1.5 1.999 2 2.499 2.5 3]';
		F=[1 1 -1 -1 1 1 -1 -1 1 1 -1 -1]';
		mode=1;
	else						% user data invocation
		t=arg1(:);	F=arg2(:);	% make sure they are columns
		if any(diff(t)<=0)
			error('all t must be strictly increasing')
		end
		if (length(t)~=length(F))
			error('t and F must be of equal length')
		end
		mode=3;
	end
	p=struct('M',2.81448e-3, ...
	   'Bf',0.0212207, ...
	   'Ks',1, ...
	   'Fi',1, ...
	   'Ff',2, ...
	   'fn',3, ...
	   'zeta',0.2, ...
	   'K',1, ...
	   'y0',2, ...
	   'yp0',0, ...
	   'A',1, ...
	   'f',3, ...
	   'mode',mode, ...
	   't',t, ...
	   'F',F, ...
	   'trivia',0,'yline',0, ...
	   'fnedit',0,'zetaedit',0,'Kedit',0, ...
	   'stepclump',zeros(11,1), ...
	   'constclump',zeros(12,1), ...
	   'genclump',zeros(11,1), ...
	   'startclump',zeros(14,1), ...
	   'steadyclump',zeros(12,1));

    a=figure('Units','normalized','Position',[0.17 0.07 0.8 0.8], ...
      'Numbertitle','off','Name','SecondOrd','Tag','SecondOrdBR549');
	set(0,'ShowHiddenHandles','on')
	wh=findobj('Tag','SecondOrdBR549');
	s=length(wh);
	if (s>1)
		set(a,'Name',['SecondOrd #',num2str(s)]);
	end
	set(0,'ShowHiddenHandles','off')
	currdir=pwd;
	homedir=pathto('mainswitch.m');
	cd(homedir)
	d=dir('fontpref.mat');
	if (length(d))
		load fontpref
		set(a,'DefaultAxesFontName',axfont.FontName, ...
			'DefaultAxesFontUnits',axfont.FontUnits, ...
			'DefaultAxesFontSize',axfont.FontSize, ...
			'DefaultAxesFontWeight',axfont.FontWeight, ...
			'DefaultAxesFontAngle',axfont.FontAngle, ...
			'DefaultUicontrolFontName',uifont.FontName, ...
			'DefaultUicontrolFontUnits',uifont.FontUnits, ...
			'DefaultUicontrolFontSize',uifont.FontSize, ...
			'DefaultUicontrolFontWeight',uifont.FontWeight, ...
			'DefaultUicontrolFontAngle',uifont.FontAngle)
	end
	cd(currdir)
	color=get(a,'Color');
	b=axes('Parent',a,'Units','normalized','XGrid','on','YGrid','on', ...
		'Position',[0.33 0.12 0.6 0.80],'box','on');
	c=uicontrol('Parent',a,'Style','text','Units','normalized', ...
	   'Position',[0.015 0.94 0.195 0.05],'BackgroundColor',color, ...
	   'String','Input Function');
	c=uicontrol('Parent',a,'Style','popupmenu','Units','normalized', ...
	   'Position',[0.015 0.88 0.195 0.05],'BackgroundColor',color, ...
	   'String',['Force Step|Constant|Line Segments|', ...
	   		'Start Harmonic|Steady Harmonic'], ...
	   'Callback','secondord popup','Value',mode);
	c=uicontrol('Parent',a,'Style','text','Units','normalized', ...
	   'Position',[0.015 0.767 0.195 0.05],'BackgroundColor',color, ...
	   'String','Parameters');
	% edit boxes for Step follow
	Medit=uicontrol('Parent',a,'Style','edit','Units','normalized', ...
	   'Position',[0.015 0.707 0.15 0.06],'String',num2str(p.M), ...
	   'callback','secondord M');
	Mlabel=uicontrol('Parent',a,'Style','text','Units','normalized', ...
	   'Position',[0.18 0.707 0.03 0.05],'BackgroundColor',color, ...
	   'String','M');
	Bfedit=uicontrol('Parent',a,'Style','edit','Units','normalized', ...
	   'Position',[0.015 0.626 0.15 0.06],'String',num2str(p.Bf), ...
	   'callback','secondord Bf');
	Bflabel=uicontrol('Parent',a,'Style','text','Units','normalized', ...
	   'Position',[0.18 0.626 0.03 0.05],'BackgroundColor',color, ...
	   'String','Bf');
	Ksedit=uicontrol('Parent',a,'Style','edit','Units','normalized', ...
	   'Position',[0.015 0.545 0.15 0.06],'String',num2str(p.Ks), ...
	   'callback','secondord Ks');
	Kslabel=uicontrol('Parent',a,'Style','text','Units','normalized', ...
	   'Position',[0.18 0.545 0.03 0.05],'BackgroundColor',color, ...
	   'String','Ks');
	Fiedit=uicontrol('Parent',a,'Style','edit','Units','normalized', ...
	   'Position',[0.015 0.464 0.15 0.06],'String',num2str(p.Fi), ...
	   'callback','secondord Fi');
	Filabel=uicontrol('Parent',a,'Style','text','Units','normalized', ...
	   'Position',[0.18 0.464 0.03 0.05],'BackgroundColor',color, ...
	   'String','Fi');
	Ffedit=uicontrol('Parent',a,'Style','edit','Units','normalized', ...
	   'Position',[0.015 0.383 0.15 0.06],'String',num2str(p.Ff), ...
	   'callback','secondord Ff');
	Fflabel=uicontrol('Parent',a,'Style','text','Units','normalized', ...
	   'Position',[0.18 0.383 0.03 0.05],'BackgroundColor',color, ...
	   'String','Ff');
	trivia=uicontrol('Parent',a,'Style','text','Units','normalized', ...
	   'Position',[0 0.03 0.21 0.2],'BackgroundColor',color, ...
	   'String',{['fn = ',num2str(p.fn)];['zeta = ',num2str(p.zeta)];
	   		['K = ',num2str(p.K)]});
	p.trivia=trivia;
	p.stepclump=[Medit;Mlabel;Bfedit;Bflabel;Ksedit;Kslabel;
			Fiedit;Filabel;Ffedit;Fflabel;trivia];
	% edit boxes for Constant mode follow
	fnedit=uicontrol('Parent',a,'Style','edit','Units','normalized', ...
	   'Position',[0.015 0.707 0.15 0.06],'String',num2str(p.fn), ...
	   'callback','secondord fn','Visible','off');
	p.fnedit=fnedit;
	fnlabel=uicontrol('Parent',a,'Style','text','Units','normalized', ...
	   'Position',[0.18 0.707 0.04 0.05],'BackgroundColor',color, ...
	   'String','fn','Visible','off');
	zetaedit=uicontrol('Parent',a,'Style','edit','Units','normalized', ...
	   'Position',[0.015 0.626 0.15 0.06],'String',num2str(p.zeta), ...
	   'callback','secondord zeta','Visible','off');
	p.zetaedit=zetaedit;
	zetalabel=uicontrol('Parent',a,'Style','text','Units','normalized', ...
	   'Position',[0.18 0.626 0.05 0.05],'BackgroundColor',color, ...
	   'String','zeta','Visible','off');
	Kedit=uicontrol('Parent',a,'Style','edit','Units','normalized', ...
	   'Position',[0.015 0.545 0.15 0.06],'String',num2str(p.K), ...
	   'callback','secondord K','Visible','off');
	p.Kedit=Kedit;
	Klabel=uicontrol('Parent',a,'Style','text','Units','normalized', ...
	   'Position',[0.18 0.545 0.03 0.05],'BackgroundColor',color, ...
	   'String','K','Visible','off');
	Aedit=uicontrol('Parent',a,'Style','edit','Units','normalized', ...
	   'Position',[0.015 0.464 0.15 0.06],'String',num2str(p.A), ...
	   'callback','secondord A','Visible','off');
	Alabel=uicontrol('Parent',a,'Style','text','Units','normalized', ...
	   'Position',[0.18 0.464 0.03 0.05],'BackgroundColor',color, ...
	   'String','A','Visible','off');
	y0edit=uicontrol('Parent',a,'Style','edit','Units','normalized', ...
	   'Position',[0.015 0.302 0.15 0.06],'String',num2str(p.y0), ...
	   'callback','secondord y0','Visible','off');
	y0label=uicontrol('Parent',a,'Style','text','Units','normalized', ...
	   'Position',[0.18 0.302 0.05 0.05],'BackgroundColor',color, ...
	   'String','y(0)','Visible','off');
	yp0edit=uicontrol('Parent',a,'Style','edit','Units','normalized', ...
	   'Position',[0.015 0.221 0.15 0.06],'String',num2str(p.yp0), ...
	   'callback','secondord yp0','Visible','off');
	yp0label=uicontrol('Parent',a,'Style','text','Units','normalized', ...
	   'Position',[0.18 0.221 0.05 0.05],'BackgroundColor',color, ...
	   'String','y''(0)','Visible','off');
	p.constclump=[fnedit;fnlabel;zetaedit;zetalabel;Kedit;Klabel;
		Aedit;Alabel;y0edit;y0label;yp0edit;yp0label];
	% return button for general mode
	rb=uicontrol('Parent',a,'Style','pushbutton','Units','normalized', ...
	   'Position',[0.015 0.015 0.15 0.06],'Visible','off', ...
	   'String','Return','Callback','secondord return');
	p.genclump=[fnedit;fnlabel;zetaedit;zetalabel;Kedit;Klabel;
		y0edit;y0label;yp0edit;yp0label;rb];
	% edit boxes for sinusoidal modes
	fedit=uicontrol('Parent',a,'Style','edit','Units','normalized', ...
	   'Position',[0.015 0.383 0.15 0.06],'String',num2str(p.f), ...
	   'callback','secondord f','Visible','off');
	flabel=uicontrol('Parent',a,'Style','text','Units','normalized', ...
	   'Position',[0.18 0.383 0.03 0.05],'BackgroundColor',color, ...
	   'String','f','Visible','off');
	p.startclump=[fnedit;fnlabel;zetaedit;zetalabel;Kedit;Klabel;
		Aedit;Alabel;fedit;flabel;y0edit;y0label;yp0edit;yp0label];
	% Response button for steady harmonic mode
	frb=uicontrol('Parent',a,'Style','pushbutton','Units','normalized', ...
	   'Position',[0.015 0.302 0.15 0.06],'Visible','off', ...
	   'String','Freq. Response','Callback','secondord Response');	
	p.steadyclump=[fnedit;fnlabel;zetaedit;zetalabel;Kedit;Klabel;
		Aedit;Alabel;fedit;flabel;frb;trivia];
	b = uimenu('Parent',a,'Label','SecondOrd','Tag','uimenu1');
	c1 = uimenu('Parent',b,'Label','Help','Enable','on','Tag','helpmenu', ...
		'Callback','secondord help');
	c2 = uimenu('Parent',b,'Label','Visible','Enable','on','Tag','onmenu', ...
		'Callback',['set(gcf,''Handlevisibility'',''on'');', ...
			'set(get(gcbo,''UserData''),''Enable'',''on'');', ...
			'set(gcbo,''Enable'',''off'')']);
	c3 = uimenu('Parent',b,'Label','Callback','Enable','off', ...
		'Callback',['set(gcf,''Handlevisibility'',''callback'');', ...
			'set(get(gcbo,''UserData''),''Enable'',''on'');', ...
			'set(gcbo,''Enable'',''off'')'],'UserData',c2);
	set(c2,'UserData',c3)
% 	c=uicontrol('Parent',a,'Style','pushbutton','Units','normalized', ...
% 	   'Position',[0.25 0.015 0.05 0.06],'String','p', ...
% 	   'Callback','p=secondord(''p'')');
	% start program
	switch (p.mode)
	case 1	
		p=doit(p);
	case 3
		set(p.stepclump,'Visible','off')
		set(p.genclump,'Visible','on')
		p=doit(p);
	end
	set(gcf,'HandleVisibility','callback')
	set(a,'UserData',p)

else
	error('Argument Error!')
end


function p=doit(p)
cla
legend off
grid on
hold on
if (p.zeta<=1)
	tau=1/(p.zeta*2*pi*p.fn);	% approx time scale for display limits
else
	tau=1/((p.zeta-sqrt(p.zeta^2-1))*2*pi*p.fn);
end
switch p.mode
case 1					% Step mode
   t=[0:0.005:1]*4*tau;
   start=p.Fi*p.K;	targ=p.Ff*p.K;
	y=secondstep(t,p.fn,p.zeta,p.K,start,0,p.Ff);
	Fh=plot([-tau 0 0 4*tau],[p.Fi p.Fi p.Ff p.Ff],'k','LineWidth',1.5);
	yh=plot([-tau t],[start y],'b','LineWidth',1.5);
	index=1:201;	crit=abs(targ-start)/10;
	rise=min(index(abs(y-targ)<crit))-1;
	settle=max(index(abs(y-targ)>crit));
	if (settle<rise) settle=rise; end
   threshr=targ+crit*sign(start-targ);
   riset=interp1(y(rise:rise+1),t(rise:rise+1),threshr);
   threshs=targ+crit*sign(y(settle)-targ);
   settlet=interp1(y(settle:settle+1),t(settle:settle+1),threshs);
	xd=[[1 1]*riset NaN [1 1]*settlet];
	yd=[threshr (start+targ)/2 NaN threshs (start+targ*2)/3];
	plot(xd,yd,'k')
	xd=[riset/2 (settlet+4*tau)/2 NaN riset/2 (settlet+4*tau)/2];
	yd=[[1 1]*targ+crit NaN [1 1]*targ-crit];
	plot(xd,yd,'k--')
	th=get(gca,'Title');
	font=get(th,'FontName');	fsz=get(th,'FontSize')+3;
	th=text(riset,(start+targ)/2,'Rise');
	set(th,'FontSize',fsz,'FontName',font,'HorizontalAlignment','right')
	th=text(settlet,(start+targ*2)/3,'Settle');
	set(th,'FontSize',fsz,'FontName',font,'HorizontalAlignment','left')
	legend([yh,Fh],'y','F')
	xlabel('Time'),	ylabel('Displacement y')
   title('Response of Mechanical System to Step of Input Force')
   res=200*10^floor(log10(4*tau));
   riset=round(riset*res)/res;
   settlet=round(settlet*res)/res;
	trivia={['fn = ',num2str(p.fn)];['zeta = ',num2str(p.zeta)];
	   	['K = ',num2str(p.K)];['Rise Time = ',num2str(riset)];
		['Settle Time = ',num2str(settlet)]};
 	set(p.trivia,'String',trivia)
case 2					% General mode
	t=[0:0.005:1]*5*tau;
	y=secondstep(t,p.fn,p.zeta,p.K,p.y0,p.yp0,p.A);
	Fh=plot([0 5*tau],[1 1]*p.A,'k','LineWidth',1.5);
	yh=plot(t,y,'b','LineWidth',1.5);
	legend([yh,Fh],'y','F')
	xlabel('t'),	ylabel('y')
	title(['Response of General Second-Order System with F=A'])
case 3					% Piecewise General mode
	Fh=plot(p.t,p.F,'k','LineWidth',1.5);
 	trange=[p.t(1) p.t(end)];	% plot the line using ode45 time basis 
	wn=(p.fn*2*pi);		C=wn^2*[p.K, -1, -2*p.zeta/wn];
	[t,r]=ode45('secondord',trange,[p.y0;p.yp0],[],C,p.t,p.F);
	y=r(:,1);
	p.yline=plot(t,y,'b','LineWidth',1.5);
	legend([p.yline,Fh],'Response','F(t)')
	xlabel('t'),	ylabel('y')
	title('Response of Second-Order System to Line Segments F(t)')
case 4					% Start Harmonic mode
 	w=2*pi*p.f;
	phi=atan2(-2*p.zeta*p.f/p.fn,1-(p.f/p.fn)^2);
 	xb=1/p.f-phi/w;
 	t=[0:0.002:1]*max([5*tau, xb]);
 	F=p.A*sin(w*t);
	[y,ys]=secondsine(t,p.fn,p.zeta,p.K,p.y0,p.yp0,p.A,p.f);
	Fh=plot(t,F,'k','LineWidth',1.5);
	sh=plot(t,ys,'b:');
	yh=plot(t,y,'b','LineWidth',1.5);
	legend([yh,sh,Fh],'Response','asymptote','F')
	xlabel('t'),	ylabel('y')
	title(['Startup of Second-Order System with F=A sin(2\pift)'])
case 5					% Steady Harmonic mode
	w=2*pi*p.f;
	B=p.K*p.A/sqrt((1-(p.f/p.fn)^2)^2+(2*p.zeta*p.f/p.fn)^2);
	phi=atan2(-2*p.zeta*p.f/p.fn,1-(p.f/p.fn)^2);
	delay=phi/w;
	xb=1/p.f-delay;
	t=[0:0.005:1]*xb;
	F=p.A*sin(w*t);
	ys=B*sin(w*t+phi);
	Fh=plot(t,F,'k','LineWidth',1.5);
	sh=plot(t,ys,'b','LineWidth',1.5);
	h=max(abs([p.A B]))*sign(B);
	xd=[-delay+[1 1.3]*1/4/p.f NaN, ...
		 [1 1]/2/p.f NaN [1 1]/2/p.f-delay 1/2/p.f+[-delay -0.1/p.f] NaN, ...
		 1/2/p.f-[1 0 1]*delay/3.5 NaN 1/2/p.f-delay+[1 0 1]*delay/3.5];
	yd=[[1 1]*B NaN, ...
		 0 -0.8*h NaN 0 -0.8*h,[h h]*-0.6 NaN, ...
		 -0.6*h+[1 0 -1]*h/20 NaN -0.6*h+[1 0 -1]*h/20];
	plot(xd,yd,'k')
	th=get(gca,'Title');
	font=get(th,'FontName');	fsz=get(th,'FontSize')+3;
	th=text(-delay+1/4/p.f*1.35,B,'B');
	set(th,'FontSize',fsz,'FontName',font)
	th=text(0.4/p.f,-0.6*h,'\beta_2');
	set(th,'FontSize',fsz,'FontName',font,'horizontalalignment','right')
	legend([sh,Fh],'Response','F')
	trivia={['B = ',num2str(B)];['M = ',num2str(B/p.K/p.A)];
		['delay = ',num2str(delay)]};
	set(p.trivia,'String',trivia)
	xlabel('t'),	ylabel('y')
	title(['Steady Response of Second-Order System to F=A sin(2\pift)'])
end

function y=secondstep(t,fn,zeta,K,y0,yp0,A)
% y=secondstep(t,fn,zeta,K,y0,yp0,A)
wn=2*pi*fn;
if (zeta>1) 		% over-damped
	sq=sqrt(zeta^2-1);
	C1=y0+(yp0/wn+y0*(zeta-sq)-K*A*(zeta+sq))/(2*sq);
	r1=wn*(-zeta+sq);
	C2=(-yp0/wn-y0*(zeta-sq)+K*A*(zeta-sq))/(2*sq);
	r2=wn*(-zeta-sq);
	y=K*A + C1*exp(r1*t) + C2*exp(r2*t);
elseif (zeta==1)	% critical damped
	C1=y0-K*A;
	r1=-zeta*wn;
	C2=yp0+(y0-K*A)*zeta*wn;
	r2=r1;
	y=K*A + C1*exp(r1*t) + C2*t.*exp(r2*t);
elseif (zeta<1)		% under-damped
	sq=sqrt(1-zeta^2);
	C1=(yp0/wn+(y0-K*A)*zeta)/sq;
	r1=-zeta*wn;
	C2=y0-K*A;
	r2=r1;
	y=K*A + C1*exp(r1*t).*sin(wn*sq*t) + C2*exp(r2*t).*cos(wn*sq*t);
end

function [y,yp]=secondsine(t,fn,zeta,K,y0,yp0,A,f)
% y=secondsine(t,fn,zeta,K,y0,yp0,A,f)
w=2*pi*f;
wn=2*pi*fn;
den=(1-(w/wn)^2)^2 + (2*zeta*w/wn)^2;
yp=K*A*((1-(w/wn)^2)*sin(w*t) - 2*zeta*w/wn*cos(w*t))/den;
if (zeta>1) 		% over-damped
	sq=sqrt(zeta^2-1);
	C1=yp0/(2*wn*sq) + y0*(zeta+sq)/(2*sq);
	C1=C1 - K*A*w*((1-(w/wn)^2)-2*zeta*(zeta+sq))/(2*wn*sq*den);
	r1=wn*(-zeta+sq);
	C2=y0 - C1 + 2*K*A*zeta*w/wn/den;
	r2=wn*(-zeta-sq);
	yh=C1*exp(r1*t) + C2*exp(r2*t);
	y=yh+yp;
elseif (zeta==1)	% critical damped
	C1=y0+2*K*A*zeta*w/wn/den;
	r1=-zeta*wn;
	C2=yp0 + y0*zeta*wn + K*A*(2*zeta^2*w - (1-(w/wn)^2)*w)/den;
	r2=r1;
	yh=C1*exp(r1*t) + C2*t.*exp(r2*t);
	y=yh+yp;
elseif (zeta<1)		% under-damped
	sq=sqrt(1-zeta^2);
	C1=1/sq*(yp0/wn + y0*zeta + K*A/den*w/wn*(2*zeta^2-1+w^2/wn^2));
	r1=-zeta*wn;
	C2=y0 + K*A*2*zeta*w/wn/den;
	r2=r1;
	yh=C1*exp(r1*t).*sin(wn*sq*t) + C2*exp(r2*t).*cos(wn*sq*t);
	y=yh+yp;
end

function dest=pathto(fname)
r=which(fname);
[tok1,r]=strtok(r,filesep);
dest=tok1;
[tok2,r]=strtok(r,filesep);
while ~isempty(r)
	tok1=tok2;
	dest=[dest,filesep,tok1];
	[tok2,r]=strtok(r,filesep);
end

% by Tim Conover 10/10/99
