%CHIRPTERN.M - This function will create a chirp signal for the Tern Path Profile
%	Inputs - (w1,w2,A,T) - start freq(Hz), stop freq, amplitude, time duration(s)
%Arthur McClung
%11/26/02
%10/12/04 - New ver3 will be written to work with VM ServoController
function chirptern(w1,w2,A,T)
%NOTE: Had to change? to 50Hz to work w/VM servo controller
f=50;
if (nargin~=4)
   w1=input('Enter the start frequency : ');
   w2=input('Enter the stop frequency : ');
   A=input('Enter the amplitude : ');
   T=input('Enter the duration : ');
end
cselect=0;
while cselect~=2
	t=0:1/f:T;
   cp=floor(A*cos(2*pi*t.*(w1+t*(w2-w1)/T)));
   tc=0:.001:T;
   cont_cp=A*cos(2*pi*tc.*(w1+tc*(w2-w1)/T));
   w=w1+t*(w2-w1)/T;
%    if length(cp)>=300
%       error(['ERR: ' num2str(length(cp)) ' points. Reduce duration . More than 300 points in profile!'])
%    end
	figure
   subplot(2,1,1), hold on, plot(tc,cont_cp,'g:');
	plot(t,cp,'b-x')
   title(['Chirp Signal [' num2str(w1) '-' num2str(w2) '] for ' num2str(T) 's at ' num2str(f) 'Hz'])
   subplot(2,1,2), plot(w,w,'r')
   xlabel('Frequency (Hz)')
	cselect=menu('Repeat?','Yes','No');
	if cselect==1
	   w1=input('Enter the start frequency : ');
	   w2=input('Enter the stop frequency : ');
	   A=input('Enter the amplitude : ');
      T=input('Enter the duration : ');
   end
end
sselect=0;
sselect=menu('Save?','Yes','No');
if sselect==1
   [nfile, pfile]=uiputfile('*.dat','Save As');
   if length(nfile)<=3
      nfile=[nfile '.dat'];
   else if strcmp(nfile(length(nfile)-3:length(nfile)),'.dat')~=1
         nfile=[nfile '.dat'];
      end
   end
   eval(['fid=fopen(''' nfile ''',''w'');'])
%   fprintf(fid,' %3d',length(cp));
   stepdur=1/f*1000;
   for i=1:length(cp)
      %Put the chirp on the 1st 3 servos, no spacing on 1st column
      for j=1:3
         fprintf(fid,'%d\t',cp(i)+128);
      end
      %Put nothing on the 4th-6th servos
      for j=1:3
         fprintf(fid,'%d\t',128);
      end
      %Hold the rest at a fixed position
      for j=1:6
         fprintf(fid,'%d\t',128);
      end
      %Had to change to \r rather than \n to work with Servo Controller program (TM -01/14/05)
      fprintf(fid,'%d\t\r',stepdur);  
   end
   fclose(fid);
   fprintf(['''' nfile ''' was saved.\n'])
end
