close all
clear all

%HydraTech Rams
%Ram Dimensions

D_Ram = 1.125;  %OD
D_Rod = 3/4;  %ID
A = 0.25*pi*D_Ram^2;
Ar = A-0.25*pi*D_Rod^2;
S = 12;  %Stroke
Length = 22.0;   %Pin to Pin Length in retracted state
R1_v = [28.2459 -5.006]';
R2_v = [25.2876 -11.3448]';

%Allen-Air Rams
%D_Ram = 1.5;  %OD
%D_Rod = 0.5;  %ID
%A = 0.25*pi*D_Ram^2;
%Ar = A-0.25*pi*D_Rod^2;
%S = 12;  %Stroke
%Length = 26.4375;   %Pin to Pin Length in retracted state
%R1_v = [32.4999 -6.2702]';
%R2_v = [29.5415 -12.6144]';



figure('Position',[ 423        1029         560         420]);
x = 0:S/100:S;
plot(x,x+Length);
xlabel('Ram Extension (in)');
ylabel('Ram Length (in)');
title([num2str(x(1)+Length) ' -> ' num2str(x(end)+Length)]);




delta_theta = 120;  %Total Angle
theta_neutral = -40 %Midpoint.
theta_step = 1;

theta = theta_neutral-delta_theta/2:theta_step:theta_neutral+delta_theta/2;



theta_neutral = theta_neutral*pi/180;

%Frame Mount Points;  Top and bottom mount
L = 7;  %inches
theta0 = 105*pi/180;

r1_f = [L*cos(theta0) ; L*sin(theta0)];

r2_f = [L*cos(theta0+pi) ; L*sin(theta0+pi)];



disp(['R1_v = (' num2str(R1_v(1)) ',' num2str(R1_v(2)) ')']);
disp(['R2_v = (' num2str(R2_v(1)) ',' num2str(R2_v(2)) ')']);


for i = 1:length(theta)
    th = theta(i)*pi/180;
    Rvf = [cos(th) -sin(th);
           sin(th)  cos(th)];
       
    r1_v(:,i) = Rvf*r1_f;   
    r2_v(:,i) = Rvf*r2_f;
    
    P1_v(:,i) = R1_v - r1_v(:,i);
    P2_v(:,i) = R2_v - r2_v(:,i);

    L1(i) = norm(P1_v(:,i));  %Pin to Pin Length
    L2(i) = norm(P2_v(:,i));  %Pin to Pin Length
    PisVol1(i) = (L1(i)-Length)*A;
    RodVol1(i) = (S-(L1(i)-Length))*Ar;
    Vol1(i) = RodVol1(i)+PisVol1(i);
    
    PisVol2(i) = (L2(i)-Length)*A;
    RodVol2(i) = (S-(L2(i)-Length))*Ar;
    Vol2(i) = RodVol2(i)+PisVol2(i);   
    
    VolA(i) = RodVol1(i)+PisVol2(i);
    VolB(i) = RodVol2(i)+PisVol1(i);

    
    F1 = A;  %Force per psi
    F2 = Ar;
    T = cross(F1*[P1_v(:,i) ; 0]/L1(i),[r1_v(:,i) ; 0]);
    Torque1(i) = T(3);
    Angle1(i) = acos(dot(F1*[P1_v(:,i) ; 0]/L1(i),[r1_v(:,i) ; 0])/(norm(F1*[P1_v(:,i) ; 0]/L1(i))*norm([r1_v(:,i) ; 0])));
    T =  cross(F2*[P2_v(:,i) ; 0]/L2(i),[r2_v(:,i) ; 0]);
    Torque2(i) = -T(3);
    Angle2(i) = acos(dot(F2*[P2_v(:,i) ; 0]/L2(i),[r2_v(:,i) ; 0])/(norm(F2*[P2_v(:,i) ; 0]/L2(i))*norm([r2_v(:,i) ; 0])));
end
    

num2str(L2(end)-Length,6)
num2str(L2(1)-(Length+S),6)

num2str(L2(1)-(Length+S) + L2(end)-Length,6)

figure('Position',[1000     314     932    1184]);
subplot(5,1,1)
plot(theta,VolA);
hold all
plot(theta,VolB);
plot(theta,VolA+VolB);
xlabel('angle (degrees)');
ylabel('Volume (in^3)')
legend('VolA','VolB','Total Volume');
title(['Cyl Diam = ' num2str(D_Ram) ' in    Rod Diam = ' num2str(D_Rod) 'in   Cyl Stroke = ' num2str(S) ' in']);
grid on;


subplot(5,1,2)
plot(theta(2:end),1./((diff(VolA)./diff(theta))*60/231));
hold all
plot(theta(2:end),1./((diff(VolB)./diff(theta))*60/231));
xlabel('angle (degrees)');
ylabel('(deg/sec)/GPM');
GPMPerDegPerSec = (diff(VolB)./diff(theta))*60/231;  %GPM per Deg/Sec
title(['Mean (deg/sec)/GPM = ' num2str(mean(1./GPMPerDegPerSec),3)  '     Max (deg/sec)/GPM = ' num2str(max(1./GPMPerDegPerSec),3) '     Min (deg/sec)/GPM = ' num2str(min(1./GPMPerDegPerSec),3) ]);
grid on;
legend('Flow In','Flow Out');

subplot(5,1,3)
plot(theta,Torque1)
hold all
plot(theta,Torque2)
plot(theta,Torque1+Torque2)
xlabel('Frame angle (degrees)');
ylabel('Torque (in-lbs/psi)')
legend('Ram1','Ram2','Ram1+Ram2');
grid on;
title(['F1 = ' num2str(1000*F1) ' lbs/kpsi'  '   ' 'F2 = ' num2str(1000*F2) ' lbs/kpsi']);

subplot(5,1,4)
plot(theta,min(Angle1*180/pi,180-Angle1*180/pi));
hold all
plot(theta,min(Angle2*180/pi,180-Angle2*180/pi));
grid on
xlabel('Frame angle (deg)');
ylabel('Ram Relative Angle');
legend('Ram1','Ram2');

subplot(5,1,5)
plot(theta,L1);
hold all
plot(theta,L2);
plot(theta,abs(L2-L1));
grid on
xlabel('Frame angle (deg)');
ylabel('Ram Length');
legend('Ram1','Ram2','Delta');


figure('Position',[1964        1077         560         420]);
hold all;
j = 1;
for i = 1:20:length(theta)
   plot( [r1_v(1,i) 0 r2_v(1,i) r1_v(1,i)],[r1_v(2,i) 0 r2_v(2,i) r1_v(2,i) ]);
   leg_str{j} = ['theta = ' num2str(theta(i))];
   j = j+1;
end
legend(leg_str);
for i = 1:20:length(theta)
   plot( [R1_v(1) r1_v(1,i)],[R1_v(2) r1_v(2,i)],'k--');
   plot( [R2_v(1) r2_v(1,i)],[R2_v(2) r2_v(2,i)],'k--');
end
plot(0,0,'ko');
plot(R1_v(1),R1_v(2),'ko');
grid on;
plot(R2_v(1),R2_v(2),'ko');
axis equal;



