%Create calibration output functions from S1darm_08

S1darm
S1darm_08
[A,B,C,D] = linmod('S1darm_08');
darmsys = ss(A,B,C,D);

%Duncan's freq series
fd=[0:1/64:2048];

calib=1./mybodesys(darmsys(3,1),fd);
calib=calib/4e3; %calibration for AS_Q in strain/count

%Open loop function
F=mybodesys(darmsys(2,2),fd);
CLG=mybodesys(darmsys(1,1),fd);
OLG=-F./CLG;

%Calculate the sensing function
S1darm
OpenS1darm_08
[A,B,C,D] = linmod('OpenS1darm_08');
darmsys = ss(A,B,C,D);
sens=1./mybodesys(darmsys(3,1),fd);
sens=sens/4e3;

%Plots
figure(1)
clf
subplot(211)
loglog(fd,abs(calib),'LineWidth',2)
title('AS\_Q Calibration (Closed Loop)')
ylabel('mag(strain/count)')
axis([.1 2048 1e-19 1e-7])
grid on

subplot(212)
semilogx(fd,angle(calib)*180/pi,'LineWidth',2)
axis([.1 2048 -180 180])
ylabel('angle(deg)')
set(gca,'YTick',[-180:90:180])
grid on

figure(2)
clf
subplot(211)
loglog(fd,abs(sens),'LineWidth',2)
title('AS\_Q Calibration (Open Loop) or Sensing Function')
ylabel('mag(strain/count')
axis([.1 2048 1e-19 1e-16])
grid on

subplot(212)
semilogx(fd,angle(sens)*180/pi,'LineWidth',2)
axis([.1 2048 -180 180])
ylabel('angle(deg)')
set(gca,'YTick',[-180:90:180])
grid on

figure(3)
clf
subplot(211)
loglog(fd,abs(OLG),'LineWidth',2)
title('Open Loop Gain')
ylabel('mag')
axis([.1 2048 1e-3 1e10])
grid on

subplot(212)
semilogx(fd,angle(OLG)*180/pi,'LineWidth',2)
axis([.1 2048 -180 180])
ylabel('angle(deg)')
set(gca,'YTick',[-180:90:180])
grid on

shg
disp('pause')
pause

disp('writing ASQCalibration.txt')
fid=fopen('ASQcalibration.txt','w');
fprintf(fid,'%s\t','Freq(Hz)');
fprintf(fid,'%s\t','Mag(strain/ct)');
fprintf(fid,'%s\n','Phase(rad)');

for i=1:length(fd)
  fprintf(fid,'%s\t',num2str(fd(i)));
  fprintf(fid,'%s\t',num2str(abs(calib(i))));
  fprintf(fid,'%s\n',num2str(unwrap(angle(calib(i)))));
end
fclose(fid)

disp('writing ASQSensing.txt')
fid=fopen('ASQSensing.txt','w');
fprintf(fid,'%s\t','Freq(Hz)');
fprintf(fid,'%s\t','Mag(strain/ct)');
fprintf(fid,'%s\n','Phase(rad)');

for i=1:length(fd)
  fprintf(fid,'%s\t',num2str(fd(i)));
  fprintf(fid,'%s\t',num2str(abs(sens(i))));
  fprintf(fid,'%s\n',num2str(unwrap(angle(calib(i)))));
end
fclose(fid)

disp('writing OpenLoopGain.txt')
fid=fopen('OpenLoopGain.txt','w');
fprintf(fid,'%s\t','Freq(Hz)');
fprintf(fid,'%s\t','Mag(strain/ct)');
fprintf(fid,'%s\n','Phase(rad)');

for i=1:length(fd)
  fprintf(fid,'%s\t',num2str(fd(i)));
  fprintf(fid,'%s\t',num2str(abs(OLG(i))));
  fprintf(fid,'%s\n',num2str(unwrap(angle(OLG(i)))));
end
fclose(fid)