
 %% zxe7 measurement precharge @430~450psi
 
mx_zxe7 = [0 0.1 0.2 0.4 0.5 0.6 0.7 0.8 0.9 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 20 25 30 35 40 45 50 55 60 65 70 75 80 85 90 95 100 105 110 115 120 125 130 135 140 145 150 155 150 145 140 135 130 125 120 115 110 105 100 95 90 85 80 75 70 65 60 55 50 45 40 35 30 25 20 15 10 8 7 6 5 4 3 2 1];
mF_zxe7 = [0 9 10 10.5 11 9.5 11 12 12.5 12.5 17.5 25.5 36.5 55 88 155 287 484 485 483 489 489 492 493 501 511 519 527 536 545 556 566 576 586 596 608 619 630 642 655 668 681 695 710 730 747 765 783 802 824 843 866 794 774 756 736 720 703 688 673 658 643 632 617 606 593 582 571 560 550 541 531 522 513 505 496 488 480 473 465 458 227 111 60 33 18 11 4 -1.5];
mV_zxe7 = [0.564 0.565 0.565 0.566 0.566 0.567 0.567 0.568 0.568 0.569 0.574 0.582 0.592 0.61 0.643 0.708 0.836 1.029 1.031 1.033 1.035 1.037 1.038 1.04 1.048 1.056 1.064 1.074 1.081 1.09 1.099 1.109 1.119 1.13 1.139 1.15 1.161 1.171 1.184 1.197 1.21 1.223 1.236 1.251 1.269 1.285 1.301 1.318 1.337 1.358 1.376 1.399 1.359 1.339 1.32 1.3 1.284 1.265 1.252 1.238 1.223 1.208 1.196 1.182 1.171 1.158 1.147 1.135 1.126 1.115 1.105 1.095 1.086 1.078 1.069 1.06 1.052 1.045 1.038 1.03 1.023 0.796 0.679 0.626 0.599 0.585 0.576 0.57 0.565];


%% ZXE5 parameters model for reference
ASpring_d_Bore = 3.5*0.0254;                         %Airspring bore [m]
ASpring_A_Bump = pi*(ASpring_d_Bore^2)/4;     %Airspring efective area in bump [m^2]
ASpring_V0_Bump_F = 0.0017 + 0.0*ASpring_A_Bump;                  %Front bump airspring volume @ design position [m^3]



%% Parameters for zxe7 model
zxe7_static_load = 6200;                                                   % load @ design height [N]
psi2pa = 6897.76;
Atm_Press = 101325;                                                   % atmospheric pressure. [Pa]
%param inputs
Air_spring_diameter_mm_zxe7 = 40;                                     % diameter of the separator piston the the air spring
Air_spring_assembly_length_mm_zxe7 = 416.85;                          % length of the air spring cylinder at assembly [mm]
Strut_diameter_mm_zxe7 = 45;                                          % strut diameter [mm]
Air_spring_assembly_pressure_zxe7 = 2232700*1.3 + Atm_Press;            % assembly pressure [Pa]
zxe7_assembly_pressure_bar = Air_spring_assembly_pressure_zxe7*1e-5;  %Air_spring_dead_volume = 29000/1000/1000/1000;                     % zxe7 B dead volume [l]
Air_spring_dead_volume = 16980/1000/1000/1000*0.8;                  % zxe7 dev [l]
T_ambient_zxe7 = 273.15 + 25;                                           % ambient temperature @ assembly [K]


%BASF jounce bumper
ASpring_BS_ON_zxe7 = 0; % jounce bumper should be included in skc file toggle
ASpring_MTM_ON_zxe7 = 1; % Metal to metal travel limiter toggle
BS_disp_zxe7 = [-100 -10 0 0.45 0.9 1.35 1.8 2.25 2.7 3.15 3.6 4.05 4.5 4.95 5.4 5.85 6.3 6.75 7.2 7.65 8.1 8.55 9 9.45 9.9 10.35 10.8 11.25 11.7 12.15 12.6 13.05 13.5 13.95 14.4 14.85 15.3 15.75 16.200001 16.65 17.1 17.549999 18 18.450001 18.9 19.35 19.799999 20.25 20.700001 21.15 21.6 22.049999 22.5 22.950001 23.4 23.85 24.299999 24.75 25.200001 25.65 26.1 26.549999 27 27.450001 27.9 28.35 28.799999 29.25 29.700001 30.15 30.6 31.049999 31.5 31.950001 32.400002 32.849998 33.299999 33.75 34.200001 34.650002 35.099998 35.549999 36 36.450001 36.900002 37.349998 37.799999 38.25 38.700001 39.150002 39.599998 40.049999]/1000+0.045;
BS_F_zxe7 = [0 0 7.183878 26.826462 46.570961 64.614174 83.746048 102.425919 122.090401 142.12944 164.033936 187.348679 211.443939 237.516815 266.134277 295.385529 326.976196 358.792145 391.300903 426.331207 461.889069 496.842682 534.99884 573.437317 612.50946 652.533081 694.179321 735.903564 778.941956 822.360474 867.649963 915.333008 962.963501 1011.434265 1061.525513 1111.864014 1162.358032 1213.845703 1264.745239 1316.958984 1369.884644 1423.057983 1475.914551 1530.207153 1582.541138 1642.628418 1699.613892 1755.055298 1817.619873 1876.328369 1938.985962 1993.249634 2057.07959 2126.115723 2197.563477 2271.828125 2348.761475 2429.903564 2511.682617 2598.811523 2691.086182 2782.875 2890.125732 2991.783936 3100.244385 3214.857178 3331.700195 3457.13623 3596.53833 3731.048584 3880.263672 4034.347656 4206.67627 4395.259766 4595.111816 4791.229004 5025.668945 5260.915527 5527.598145 5841.836426 6153.01709 6551.25293 6935.905273 7414.320801 7939.495605 8588.09082 9306.674805 10157.59961 11205.34668 12499.7627 14104.81348 16101.71191];


% calculations
Air_spring_piston_area_m3_zxe7 = (Air_spring_diameter_mm_zxe7/1000/2)^2*pi;  % area of the floater piston
Air_spring_assembly_volume_m3 = Air_spring_assembly_length_mm_zxe7/1000*Air_spring_piston_area_m3_zxe7+Air_spring_dead_volume;
Strut_piston_area_m2_zxe7 = (Strut_diameter_mm_zxe7/1000/2)^2*pi;            % area for the strut piston
 
% calculation of required air spring piston position for design load.
Air_spring_nominal_pos_mm_zxe7 = Air_spring_assembly_length_mm_zxe7 - (Air_spring_assembly_volume_m3/Air_spring_piston_area_m3_zxe7*(1 - Air_spring_assembly_pressure_zxe7/(zxe7_static_load/Strut_piston_area_m2_zxe7 + Atm_Press)))*1000;
Air_spring_nominal_volume_m3_zxe7 = Air_spring_nominal_pos_mm_zxe7/1000*Air_spring_piston_area_m3_zxe7 + Air_spring_dead_volume;
ASpring_P0_Bump_pa_zxe7 = Air_spring_assembly_pressure_zxe7*Air_spring_assembly_volume_m3/Air_spring_nominal_volume_m3_zxe7;
ASpring_V0_Bump_m3_zxe7 = Air_spring_nominal_volume_m3_zxe7;

%% isothermal spring curve
k = 1;
x = -0.16:0.001:0.16;
AtmF = Atm_Press*Strut_piston_area_m2_zxe7;
pisothermal = ASpring_P0_Bump_pa_zxe7*ASpring_V0_Bump_m3_zxe7.^k./(ASpring_V0_Bump_m3_zxe7 - x.*Strut_piston_area_m2_zxe7).^k;
Fisothermal = pisothermal*Strut_piston_area_m2_zxe7 - AtmF;
%% adiabatic spring curve
k = 1.4;
padiabatic = ASpring_P0_Bump_pa_zxe7*ASpring_V0_Bump_m3_zxe7.^k./(ASpring_V0_Bump_m3_zxe7 - x.*Strut_piston_area_m2_zxe7).^k;
Fadiabatic = padiabatic*Strut_piston_area_m2_zxe7 - AtmF;
reboundlimit = (Air_spring_assembly_volume_m3 - Air_spring_nominal_volume_m3_zxe7)/Strut_piston_area_m2_zxe7 + 0.0025;
reboundlimitgain = 0.5*tanh((x + reboundlimit)*1000) + 0.5;
Fadiabatic = Fadiabatic.*reboundlimitgain + x*0.0001;
Fisothermal = Fisothermal.*reboundlimitgain + x*0.0001;
% figure(1)
% hold on
% plot(x*1000,Fadiabatic,'--')
Fadiabaticzxe5 = ASpring_A_Bump*(zxe7_static_load/ASpring_A_Bump + Atm_Press)*ASpring_V0_Bump_F.^k./(ASpring_V0_Bump_F-x.*ASpring_A_Bump).^k - (Atm_Press*ASpring_A_Bump);
% plot(x*1000,Fadiabaticzxe5)
% plot(x*1000,Fisothermal)
% plot([-1 1]*160,[1 1]*Air_spring_assembly_pressure_zxe7*Strut_piston_area_m2_zxe7-AtmF,'linewidth',2)
%reboundlimit=(Air_spring_assembly_volume_m3-Air_spring_nominal_volume_m3_zxe7)/Strut_piston_area_m2_zxe7;
reboundforceadiabatic = Strut_piston_area_m2_zxe7*ASpring_P0_Bump_pa_zxe7*ASpring_V0_Bump_m3_zxe7.^k./(ASpring_V0_Bump_m3_zxe7 + reboundlimit.*Strut_piston_area_m2_zxe7).^k - AtmF;
% plot(-reboundlimit*1000,reboundforceadiabatic,'o')
% measurment_offset = 90;
% plot(mx_zxe7-measurment_offset,mF_zxe7*9.82)
% plot(mx_zxe7-measurment_offset,(mV_zxe7*62.5-35.15625)*1e5*Strut_piston_area_m2_zxe7)

% %% run simulation
% simout= sim('zxe7_struttest2.slx');
% 
% plot(-ZXE7_sim_x.data*1000,ZXE7_sim_F.data)

% grid on
% title(['spring curve comparison @',num2str(static_load), ' N'])
% xlabel('strut displacement [mm]')
% ylabel('Force [N]')
% ratecalc(x,Fadiabatic,'zxe7 ',0);
% ratecalc(x,Fadiabaticzxe5,'zxe5 ',-1000);
% text(-50,10000,[num2str(Air_spring_assembly_pressure_zxe7/psi2pa),' psi assy pressure'])
% legend('zxe7','zxe5','zxe7 isothermal','zxe7 min load isothermal','zxe7 min load adiabatic','ZXE7 measure','ZXE7 measure P x A')
%%
% Hydraulic supply model
% AR area
RA_A_extend_zxe7 = pi*0.0225^2;
RA_A_retract_zxe7 = (ICLS_A_extend - pi*(0.0225/2)^2)*0;

% pump controls
Pump_on_zxe7 = 1;       %toggle the hydraulic pumps on/off
ASC_MPU_duty_gain = 1.0; %

% accumulator data
acc_p0_zxe7 = 50;      %accumulator gas preload pressure [bar]
Vdead_zxe7 = 0;   %Accumulator vead volume   [m^3]
VC_zxe7 = 0.0014;      %Fluid chamber capacity  [m^3]

% pump data

%ZF measured data
press_ZF_zxe7 = [10 20 30 40 50 60 70 80 90 100 110 113 125]; %pressure [bar]
flow_ZF_zxe7 = [9.5 9.5 9.5 9.5 9.5 8.3 7.4 6.7 6 5.3 4.8 4.5 0]; %measured flow [lpm]
maxrpm_ZF_zxe7 = 5931;  % max pump speed [rpm]

Hyd_leak_rate_zxe7 = 1/60*0.0002/170*0; % m^3/s
Hyd_dump_rate_zxe7 = 0.1*0.001; % accumulator dump rate m^3/bar

F_acc_V0 = 0;     % Front accumulator starting volume
R_acc_V0 = 0;
F_pump_start_state = 1;     % If the pump is running at the start of the simulation
R_pump_start_state = 1;



%% ASC sensor resolution
cps_res = 0.1955/1000; % resolution of the signal in [m]
p_res = 0.25 *100000; % resolution of pressure signal [Pa]
RA_res = 0.07816/1000; % resolution of Actuator [m]


%BASF jounce bumper
ASpring_BS_ON_zxe7 = 0; % jounce bumper should be included in skc file
BS_disp_zxe7 = [-100 -10 0 0.45 0.9 1.35 1.8 2.25 2.7 3.15 3.6 4.05 4.5 4.95 5.4 5.85 6.3 6.75 7.2 7.65 8.1 8.55 9 9.45 9.9 10.35 10.8 11.25 11.7 12.15 12.6 13.05 13.5 13.95 14.4 14.85 15.3 15.75 16.200001 16.65 17.1 17.549999 18 18.450001 18.9 19.35 19.799999 20.25 20.700001 21.15 21.6 22.049999 22.5 22.950001 23.4 23.85 24.299999 24.75 25.200001 25.65 26.1 26.549999 27 27.450001 27.9 28.35 28.799999 29.25 29.700001 30.15 30.6 31.049999 31.5 31.950001 32.400002 32.849998 33.299999 33.75 34.200001 34.650002 35.099998 35.549999 36 36.450001 36.900002 37.349998 37.799999 38.25 38.700001 39.150002 39.599998 40.049999]/1000+0.045;
BS_F_zxe7 = [0 0 7.183878 26.826462 46.570961 64.614174 83.746048 102.425919 122.090401 142.12944 164.033936 187.348679 211.443939 237.516815 266.134277 295.385529 326.976196 358.792145 391.300903 426.331207 461.889069 496.842682 534.99884 573.437317 612.50946 652.533081 694.179321 735.903564 778.941956 822.360474 867.649963 915.333008 962.963501 1011.434265 1061.525513 1111.864014 1162.358032 1213.845703 1264.745239 1316.958984 1369.884644 1423.057983 1475.914551 1530.207153 1582.541138 1642.628418 1699.613892 1755.055298 1817.619873 1876.328369 1938.985962 1993.249634 2057.07959 2126.115723 2197.563477 2271.828125 2348.761475 2429.903564 2511.682617 2598.811523 2691.086182 2782.875 2890.125732 2991.783936 3100.244385 3214.857178 3331.700195 3457.13623 3596.53833 3731.048584 3880.263672 4034.347656 4206.67627 4395.259766 4595.111816 4791.229004 5025.668945 5260.915527 5527.598145 5841.836426 6153.01709 6551.25293 6935.905273 7414.320801 7939.495605 8588.09082 9306.674805 10157.59961 11205.34668 12499.7627 14104.81348 16101.71191];
% %controller
ICLS_Up_Travel_zxe7 = 0.08;
ICLS_Down_Travel_zxe7 = -0.08;
ICLS_nominal_position_mm = 0;                                               % nominal actuator position [mm]
Init_leveling_disp_mm = 4.2;                                                % initial leveling displacement
cp_ICLS_up_travel_zxe7 = ICLS_Up_Travel_zxe7*1000;                          %Up travel of ICLS [mm]
cp_ICLS_down_travel_zxe7 = ICLS_Down_Travel_zxe7*1000;                       %Down travel of ICLS [mm]
% 
%



%% skyhook reference controller params
skyhook_filt_low = 0.01;
skyhook_filt_high = 0.1;
skyhook_mpc_iterations = 20;
skyhook_pitch_rate_trigger_val = 2;
skyhook_heave_rate_trigger_val = 0.1;
skyhook_trigger_duration_cycles = 15;
skyhook_pitch_gain = 0.1;
skyhook_heave_gain = 0.1;
%%

function rate = ratecalc(x, F,steng, offset)
rate = (F(181) - F(141))/(x(181) - x(141));
plot([x(181) x(141)]*1000,[F(181) F(141)],'*-');
text(x(181)*1000 + 10,F(181)+offset,[steng,num2str(rate/1000),' N/mm']);
end