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

initial_angle = deg2rad(5)

% weights [kg]
M_wheel_alone = 0.012;
M_motor= 0.011;
M_wheel = M_wheel_alone
M_body=0.232

% radius of wheel [m]
r = 0.032
% distance from center of wheel and center of mass [m]
l = 0.035

% inertia
I_wheel = (M_wheel*(r^2)) %placeholder
I_body = (M_body*(l^2))/3 %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

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)



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, X, 'IgnoreAnalyticConstraints', true)
Leq2 = subs(Leq2, X, tmp)
Theta = solve(Leq2, Theta, 'IgnoreAnalyticConstraints', true)
F = Theta/Tau
F = simplify(F)

% values copied by hand
%num = [7205759403792793600000000*427 7205759403792793600000000*78125];
%den = [-231546606070548106228128 -213320502349782562255859375 128573898427475961600000000 44843017029672140625000000000]
%w = tf(num,den)
w = sym2tf(F)
sys = zpk(w)


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

disp(['Kd < ', num2str(-abs(p3/k))])
kd = -0.006
kponkd=abs(p1+p2)
kionkd=abs(p1*p2)
kp = kponkd * kd
ki = kionkd * kd

res = sim('self_balance_nonlinear')