close all
clear all

%load('2022.01.28T16.46.31_combined.mat');

%filename = 'Test5_ConstantForce_MultiSineCurr_sf7.5/Test5_ConstantForce_MultiSineCurr_sf7.5_CompleteResults';
%filename = 'Test6_MultiSinPos_sf7.5_MultiSineCurr_sf2.5/Test6_MultiSinPos_sf7.5_MultiSineCurr_sf2.5_CompleteResults';
%filename = '1-28-2022/2022.01.28T16.46.31_combined';
%filename = '7-26-2021/TestAfterAssembly_1_CombinedData';
filename = '2022.01.28T16.46.31_combined.mat';

load([filename '.mat']);

seconds = (Time_DateNum-Time_DateNum(1))*24*3600;
TM_Speed = [diff(TM_Range)./diff(seconds)  0];
windowSize = 20;
TM_Speed_Filt = filtfilt(ones(1,windowSize)/windowSize,1,TM_Speed);

iMax = 6;
i = 1;
figure
ah(i) = subplot(iMax,1,i); i = i+1;
plot(seconds,TM_Speed);
hold all;
plot(seconds,TM_Speed_Filt);


ylabel('Speed (in/s)');

ah(i) = subplot(iMax,1,i); i = i+1;
plot(seconds,TM_LowerHydPressure,seconds,TM_UpperHydPressure)
legend('Lower Hyd Pressure (psig)','Upper Hyd Pressure (psig)');

ah(i) = subplot(iMax,1,i); i = i+1;
plot(seconds,SC_LowerPressure,seconds,SC_UpperPressure)
legend('Lower Pneumatic Pressure (psia)','Upper Pneumatic Pressure (psia)');

ah(i) = subplot(iMax,1,i); i = i+1;
plot(seconds,PC_RPM);
ylabel('RPM');

ah(i) = subplot(iMax,1,i); i = i+1;
plot(seconds,PC_TargCurr,seconds,PC_WindingCurrent,seconds,PC_BattCurr,seconds,PC_LoadCurr,seconds,PC_BiasCurrent);
ylabel('Amps');
legend('Target Current','Winding Current','Batt Curr','Load Curr','BiasCurrent');

ah(i) = subplot(iMax,1,i); i = i+1;
plot(seconds,PC_Voltage);
ylabel('Volts');
legend('PC_Voltage');

linkaxes(ah,'x');

ts = 180 %seconds
tf = 1750 %seconds
idx = 50*ts:50*tf;
%seconds = seconds-seconds(idx(1));

Spd = TM_Speed_Filt(idx);
Pos = cumtrapz(seconds(idx)-seconds(idx(1)),Spd)+TM_Range(idx(1));


fileID = fopen([filename '.exp'],'w');
fprintf(fileID,"#time(s),Pos(in),Spd(in/s),RPM,LowerHydPress(psig),UpperHydPress(psig),VBus,WindCurr(A),BattCurr(A),LoadCurr(A),Scale,Retract,LowerSpringPressure(psia),UpperSpringPressure(psia),TargetWindingCurrent(A),PC_Status,TestMachineForce(lbs),BiasCurrent(A)\n");
for i = 1:length(idx)
    if(mod(i-1,round(length(idx)/100)) == 0)
    disp([num2str(100*i/length(idx),'%.1f') '%']);
    end
  fprintf(fileID,'%6.2f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %d %.3f\n',seconds(idx(i))-seconds(idx(1)),Pos(i),Spd(i),PC_RPM(idx(i)),TM_LowerHydPressure(idx(i)),TM_UpperHydPressure(idx(i)),PC_Voltage(idx(i)),PC_WindingCurrent(idx(i)),PC_BattCurr(idx(i)),PC_LoadCurr(idx(i)),PC_Scale(idx(i)),PC_Retract(idx(i)),SC_LowerPressure(idx(i)),SC_UpperPressure(idx(i)),PC_TargCurr(idx(i)),PC_Status(idx(i)),TM_Force(idx(i)),PC_BiasCurrent(idx(i)));
end
fclose(fileID);