close all
clear all

data = load('Full System Test.DAT');  %%"Inch","lbf","PSI","PSI","Volt","degC","degC","cm"
dt = .025;


pos = data(:,1);
tension = data(:,2);
upper_press = data(:,3);
lower_press = data(:,4);
upper_temp = data(:,6);
lower_temp = data(:,7);
rangefinder = data(:,8);

time = 0:dt:dt*(length(pos)-1);

clear data;

vel_raw = [0; diff(pos)/dt];
vel = filtfilt(ones(12,1)/12,1,vel_raw);

pos = pos/39.4;  %convert to meters.
vel = vel/39.4;  %convert to m/s.

idx_T100 = 10000:20000;
idx_T20 = 1:6000;
idx_T10 = 23000:26600;
idx_T6p6 = 27400:33600;

fh1 = figure('Position',[196    79   928   661],'PaperOrientation', 'landscape','PaperPositionMode','manual','PaperType','usletter','PaperPosition',[0 0 11 8.5]); 
ah1 = subplot(3,1,1);
plot(pos(idx_T100),tension(idx_T100),pos(idx_T20),tension(idx_T20),pos(idx_T10),tension(idx_T10),pos(idx_T6p6),tension(idx_T6p6))
set(gca,'FontSize',12);
legend('A = 1m,T = 100s,Vmax = 0.063 m/s','A = 1m,T = 20s,Vmax = 0.31 m/s','A = 1m,T = 10s,Vmax = 0.63 m/s','A = 1m,T = 6.6s,Vmax = .95 m/s','Location','NorthWest');
xlabel('Position (inches)');
ylabel('Tension (lbs)');
title('Measured Tension for Spring and PTO together - Jan 13, 2011');


%Damping behavior of PTO only (from test machine measurments, not including spring and other drag effects)
b=14000;  %N/(m/s)^2  Quadractic damping due to PTO, from measurements of PTO only.

F_spring = tension-b*vel.*abs(vel)/4.45;
ah2 = subplot(3,1,2);
plot(pos(idx_T100),F_spring(idx_T100),pos(idx_T20),F_spring(idx_T20),pos(idx_T10),F_spring(idx_T10),pos(idx_T6p6),F_spring(idx_T6p6));
set(gca,'FontSize',12);
legend('A = 1m,T = 100s,Vmax = 0.063 m/s','A = 1m,T = 20s,Vmax = 0.31 m/s','A = 1m,T = 10s,Vmax = 0.63 m/s','A = 1m,T = 6.6s,Vmax = .95 m/s','Location','NorthWest');
xlabel('Position (meters)');
ylabel('Tension (lbs)');
title('Measured Tension due to Spring (with PTO damping force (b=14000N/(m/s)^2) subtracted');



%Modeled spring tension (perfect spring force modified by "stiction" contribution.
k = 1350;  %lbs/m

A = 250;
B = 500;
C = 10;
gamma = 3;
vv = -1:.01:1;
figure('Position',[196    79   928   661],'PaperOrientation', 'landscape','PaperPositionMode','manual','PaperType','usletter','PaperPosition',[0 0 11 8.5]); 
plot(vv,+StictionForce(vv,A,B,C,gamma));
set(gca,'FontSize',12);
xlabel('Velocity (m/s)');
ylabel('Stiction Force (lbs)');
title('Stiction Force model');

print('-dpdf','StictionModel');
print('-dpng','StictionModel');
print('-dill','StictionModel');



F_spring = 400+k*pos+StictionForce(vel,A,B,C,gamma);

figure(fh1)
ah3 = subplot(3,1,3);
plot(pos(idx_T100),F_spring(idx_T100),pos(idx_T20),F_spring(idx_T20),pos(idx_T10),F_spring(idx_T10),pos(idx_T6p6),F_spring(idx_T6p6));
legend('A = 1m,T = 100s,Vmax = 0.063 m/s','A = 1m,T = 20s,Vmax = 0.31 m/s','A = 1m,T = 10s,Vmax = 0.63 m/s','A = 1m,T = 6.6s,Vmax = .95 m/s','Location','NorthWest');
xlabel('Position (meters)');
ylabel('Tension (lbs)');
title('Tension modeled as a spring force plus a stiction force that depends on velocity only');

linkaxes([ah1 ah2 ah3],'x');


print('-dpdf','MeasuredTension');
print('-dpng','MeasuredTension');
print('-dill','MeasuredTension');



idx = [idx_T100 idx_T20 idx_T10 idx_T6p6];
figure('Position',[196    79   928   661],'PaperOrientation', 'landscape','PaperPositionMode','manual','PaperType','usletter','PaperPosition',[0 0 11 8.5]); 
plot(pos(idx),tension(idx),pos(idx),F_spring(idx)+b*vel(idx).*abs(vel(idx))/4.45)
set(gca,'FontSize',12);
xlabel('Position (inches)');
ylabel('Tension (lbs)');
title('Modeled Tension versus Measured Tension');
legend('Measured Tension','Modeled Tension as quadratic damping and stiction force');


print('-dpdf','PTO_ForceComparison');
print('-dpng','PTO_ForceComparison');
print('-dill','PTO_ForceComparison');