% friction
b = 0.1
% acceleration of gravity
g = 9.81
% saturation on torque [Nm]
sat = 0.7

dt = 0.001

initial_angle = deg2rad(10)

% weights [kg]
M_wheel_alone = 0.03;
M_motor= 0.35;
M_wheel = M_wheel_alone;
M_body=0.15 + 2*M_motor

% radius of wheel [m]
r = 0.0475
% distance from center of wheel and center of mass [m]
l = 0.03

% inertia
I_wheel = (M_wheel*2)*(r^2) %placeholder
I_body = (M_body-M_motor*2)*(l^2) %placeholder

% commonly seen coeff
coeff_Ib_mbl = I_body + M_body*(l^2)
coeff_mb_Iw_mw = M_body + 2*I_wheel/(r^2) + 2*M_wheel
coeff_mb_l = M_body*l
coeff_mb2_l2 = coeff_mb_l^2

% the equations of the model
syms theta(t) x(t) tau(t)
eq1 = diff(x(t),t,2)*coeff_mb_Iw_mw + coeff_mb_l*diff(theta(t), t, 2) == tau(t)/r - diff(x(t), t, 1)*b/(r^2)
eq2 = diff(theta(t), t, 2)*(I_body+M_body*(l^2)) + coeff_mb_l*diff(x(t), t, 2) - g*coeff_mb_l*theta(t) == -tau(t)


%{
disp('method 1 - substituition in time')
syms thetaddot xddot torque
eq1_1 = subs(eq1, [diff(theta, t, 2) diff(x, t, 2) tau], [thetaddot xddot torque])
eq2_1 = subs(eq2, [diff(theta, t, 2) diff(x, t, 2) tau], [thetaddot xddot torque])
thetaddot_partial = solve(eq1_1, thetaddot, 'IgnoreAnalyticConstraints', true)
xddot_partial = subs(eq2_1, thetaddot, thetaddot_partial)
xddot_sol = solve(xddot_partial, xddot, 'IgnoreAnalyticConstraints', true)
thetaddot_partial = subs(eq1_1, xddot, xddot_sol)
thetaddot_sol = solve(thetaddot_partial, thetaddot, 'IgnoreAnalyticConstraints', true)

xddot = vpa(xddot_sol, 3)
thetaddot = vpa(thetaddot_sol ,3)
%}


% obtain a transfer function angle / angular velocity
disp('method 2 - substitution in laplace')
Leq1 = laplace(eq1);
Leq1 = subs(Leq1, [subs(diff(theta, t), t, 0) subs(diff(x, t), t, 0)], [0 0]);
Leq1 = subs(Leq1, [theta(0) x(0)], [0 0]);

Leq2 = laplace(eq2);
Leq2 = subs(Leq2, [subs(diff(theta, t), t, 0) subs(diff(x, t), t, 0)], [0 0]);
Leq2 = subs(Leq2, [theta(0) x(0)], [0 0]);

syms Theta X Tau s
Leq1 = subs(Leq1, [laplace(theta(t),t,s) laplace(x(t),t,s) laplace(tau(t),t,s)],[Theta X Tau])
Leq2 = subs(Leq2, [laplace(theta(t),t,s) laplace(x(t),t,s) laplace(tau(t),t,s)],[Theta X Tau])

%tmp = solve(Leq1, Tau, 'IgnoreAnalyticConstraints', true)
tmp = solve(Leq1, Tau)
Leq2 = subs(Leq2, Tau, tmp)
%Theta = solve(Leq2, Theta, 'IgnoreAnalyticConstraints', true)
Theta = solve(Leq2, Theta)
syms Phi
Theta = subs(Theta, s*X, Phi)
F = simplify(Theta/Phi)

w = sym2tf(F)
sys = zpk(w)

%syms Kp Ki Kd
% ideal continous time pid
%pid = tf([1 Kp/Kd Ki/Kd],[1 0])

p1 = sys.P{1}(1)
p2 = sys.P{1}(2)
z1 = sys.Z{1}
k = sys.K

absp1 = abs(p1);
absp2 = abs(p2);
absz1 = abs(z1);
absk = abs(k);

upper_limit = 1/absk
lower_limit = absp1 / (absk * (absz1 - absp2));

disp([num2str(lower_limit), ' < Kd < ', num2str(upper_limit)])
kd = 0.02
kponkd=abs(p1)-abs(p2)
kionkd=-absp1*absp2
kp = kponkd * kd
ki = kionkd * kd

%rlocus(openloop)
%{
%}

res = sim('self_balance_linear_set_state_parameters.slx')

testangle = 10