% initialize and execute two-boom dynamic and control sim...
clear
close all

R2D = 180/pi;

tsample = .01; % simulink sample time
tstop   = 100; % similink stop times


%**************************************************
%  Self-centering suspension plant model parameters
%**************************************************

L  = 20;    % half-boom length (m)
m  = 1;  % half-boom mass   (kg)

% observed rise-time of self-centering center section when you pull
% down on the boom tip.
tr = 2; % second

% undamped natural frequency estimate from rise time: tr=1.8/wn;
wn = 1.8/tr;

Jt = m*L^2/3; % moment of inertia - same for entire boom about center AND
Jb = Jt;      % for half-boom about end;

% esitmate self-centering suspension spring constant.
k = Jt*wn^2;

% specify damping ratio
d = 1;

% compute resultant damping coefficient 
b = d*2*sqrt(k*Jt);

%**************************************************
%  Compensator - simple proportional gain here
%**************************************************
Kp = 1;
cynlinderSign=-1; % cylinder retraction, or decrease in
                  % relative angle between center and
                  % boom section represents boom raise
gnuml=Kp*cynlinderSign; 
gnumr=Kp*cynlinderSign;
gden=1;

%**************************************************
%  Regulate to zero height error
%**************************************************
Yref = 0;
Yrefl=Yref;
Yrefr=Yref;

%**************************************************
%  Initial boom angles relative to center section:
%      alpha and beta
%**************************************************
ic = 0;  
a0 = ic*pi/180;  % a positive a0 places tip below zero
b0 = 0;%ic*pi/180;

%**************************************************
%  Ground-induced chassis roll model
%**************************************************
%roll    = 1; %degrees
%tramp   = 1; % seconds: time to execute roll
%rateamp = roll/tramp*pi/180; % amplitude of the roll rate pulse input
dtip     = 1; % meters you desire an uncompensated boom tip to go up or down on the side-hill disturbance.
ddur    = .1; % seconds you want the side-slop disturbance to act.
bdangle  = dtip/L; % angle uncompesated boom would settle at for the side-hill disturbance.
drate = bdangle/ddur;  % disturbance rate: degrees-per-second sideslope application to the chassis
dstart = 1;   % disturbance start time
dperiod = tstop/2;
dpw = ddur/dperiod*100;

%**************************************************
%  Slew limit on right- and left-boom
%**************************************************
tipMaxRate = 1000;  % meter per second, observed limit
slewlim = tipMaxRate/L;    % converted to radians-per-second at pivot

%**************************************************
%  deadzone
%**************************************************
rdz = 0;
ldz = rdz;


SensorDelay = 0; 

RightSensorDelay = SensorDelay;
LeftSensorDelay  = SensorDelay;

ActuatorDelay = 0;
RightActuatorDelay = ActuatorDelay;
LeftActuatorDelay  = ActuatorDelay;

%**************************************************
%  Run Simulation
%**************************************************
sim('boomsim', tstop);


%**************************************************
%  Plot Results
%**************************************************

% plot results
theta = get(logsout,'theta')
alpha = get(logsout,'alpha')
beta  = get(logsout,'beta')
lerr  = get(logsout,'lerr')
rerr  = get(logsout,'rerr')
ul  = get(logsout,'ul')
ur  = get(logsout,'ur')
Yleft  = get(logsout,'Yleft')
Yright  = get(logsout,'Yright')
disturbance  = get(logsout,'disturbance')
disturbAngle  = get(logsout,'disturbAngle')


tm = theta.Values.Time;
th = theta.Values.Data*R2D;
a  = alpha.Values.Data*R2D;
b  = beta.Values.Data*R2D;
le = lerr.Values.Data;
re = rerr.Values.Data;
cl = ul.Values.Data*R2D;
cr = ur.Values.Data*R2D;
yl = Yleft.Values.Data;
yr = Yright.Values.Data;
din= disturbance.Values.Data*180/pi;
dangle= disturbAngle.Values.Data*180/pi;

scrsz = get(0,'ScreenSize');
scrpos = [10 scrsz(4)/8 3*scrsz(3)/4 3*scrsz(4)/4];

str = sprintf('Aero sim tr=%2.1f  d=%3.1f Kp=%4.1f\n', tr, d, Kp);
figure('Name','center angles','NumberTitle','off','Position', scrpos)
plot(tm,th,'r-+',tm,a,'g*',tm,b,'b',tm,din,'k',tm,dangle,'b:')
legend('center','right(alpha)','left(beta)','disturbance rate','accumulated disturbance angle');
ylabel('boom angles(degrees)');
title(str)

figure('Name','boom angles','NumberTitle','off','Position', scrpos)
subplot(2,2,1)
plot(tm,th,'r+',tm,a,'g*',tm,b,'b')
legend('center(theta)','right(alpha)','left(beta)');
ylabel('boom angles relative to aft-to-front centerline (degrees)')
title(str)

subplot(2,2,2)
plot(tm,re*100,'g-*',tm,le*100,'b-+')
legend('right (R_e_r_r)','left (L_e_r_r)')
xlabel('time (s)')
ylabel('tip error (cm)');

subplot(2,2,3)
plot(tm,re*100,'g*',tm,le*100,'b')
legend('right (R_e_r_r)','left (L_e_r_r)')
xlabel('time (s)')
ylabel('tip error (cm)');
%axis([0 Tsim -4 4])

subplot(2,2,4)
plot(tm,cr,'g*',tm,cl,'b')
legend('right (u_r)','left (u_l)');
xlabel('time (s)')
ylabel('command (deg/sec)');


figure('Name','tip error','NumberTitle','off','Position', scrpos)
plot(tm,re*100,'g-*',tm,le*100,'b-+')
legend('right (R_e_r_r)','left (L_e_r_r)')
xlabel('time (s)')
ylabel('tip error (cm)');
title(str)

figure('Name','tip position','NumberTitle','off','Position', scrpos)
plot(tm,yr*100,'g-*',tm,yl*100,'b-+')
legend('right tip','left tip')
xlabel('time (s)')
ylabel('tip position(cm)');
title(str)


