%
% Chapter 2, Example 2.2, pages 39-39
% Optimal Reference Shaping for Dynamical Systems
% Theory and Applications
% -Tarunraj Singh
%
% Example for PPD and PPMD controllers
%
function crane_sim
%
close all

m1 = 1150;
m2 = 8000;
L = 10;
g = 9.81;

k1 = 600;
k2 = 100;
Kmat = [k1 0 k2 0];
M = [(m1+m2) m2*L; m2*L m2*L*L];
K = [0 0;0 m2*L*g];

A = [zeros(2) eye(2);-inv(M)*K zeros(2)];
B = [0;0;inv(M)*[1;0]];
Ac = A-B*Kmat;
Bc = B*k1;
C = [1 L 0 0];
D = [0];
sys = ss(Ac,Bc,C,D);
step(sys);

[Wn,Zeta,P] = damp(sys);

T1 = pi/abs(imag(P(1,1)));
temp = exp(Zeta(1)*pi/sqrt(1-Zeta(1)^2));
A0 = temp/(1+temp);
A1 = 1/(1+temp);

Avec = [A0 A1 0];
Tvec = [0 T1 T1*2.5];

T2 = pi/abs(imag(P(3,1)));
temp = exp(Zeta(3)*pi/sqrt(1-Zeta(3)^2));
A2 = temp/(1+temp);
A3 = 1/(1+temp);

Avec = [A0*A2 A0*A3 A1*A2 A1*A3 0];
Tvec = [0 T2 T1 T2+T1 (T2+T1)*1.5];

sysss = ss(Ac,Bc,C,D);
[y,t,x] = vansim(sysss,Avec,Tvec,[0;0;0;0]);
figure(2) 
plot(t,y); xlabel('Time'); ylabel('Pendulum Displacement')
hold on

% Shaped Reference Profile
figure(1)
Asum = [Avec(1) sum(Avec(1:2)) sum(Avec(1:3)) sum(Avec(1:4)) sum(Avec(1:5))]
stairs(Tvec,Asum,'r--')
xlabel('Time'); ylabel('Shaped Reference')
hold on

x0 = [5 8 0.3 0.4 0.3]; % Generates a solution
xopt = fmincon(@cost,x0,[],[],[],[],[0 0 0 0 0],[20 20 1 1 1],@ncon,[],P);
[C,Ceq] = ncon(xopt,P)

Avec2 = [xopt(3) xopt(4) xopt(5) 0];
Tvec2 = [0 xopt(1) xopt(2) xopt(2)*1.5];
[y,t,x] = vansim(sysss,Avec2,Tvec2,[0;0;0;0]);

figure(2)
plot(t,y,'--r'); xlabel('Time'); ylabel('Pendulum Displacement')

% Shaped Reference Profile
figure(1)
Asum2 = [Avec2(1) sum(Avec2(1:2)) sum(Avec2(1:3)) sum(Avec2(1:4))]
stairs(Tvec2,Asum2)
xlabel('Time'); ylabel('Shaped Reference')
%keyboard

%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function f = cost(x,P);
f = x(2);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [C,Ceq] = ncon(x,P);
T1 = x(1);
T2 = x(2);
A0 = x(3);
A1 = x(4);
A2 = x(5);

s1 = P(1);
s2 = P(3);

Temp = A0 + A1*exp(-s1*T1) + A2*exp(-s1*T2);
Ceq(1) = real(Temp);
Ceq(2) = imag(Temp);

Temp = A0 + A1*exp(-s2*T1) + A2*exp(-s2*T2);
Ceq(3) = real(Temp);
Ceq(4) = imag(Temp);
Ceq(4) = A0 + A1 + A2 - 1;

C(1) = -T1;
C(2) = -(T2-T1);
C(3) = -A0;
C(4) = -A1;
C(5) = -A2;