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

inital_angle = deg2rad(1)

% 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)
xddot_partial = subs(eq2_1, thetaddot, thetaddot_partial)
xddot = simplify(xddot_partial)


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.012
kponkd=abs(p1+p2)
kionkd=abs(p1*p2)
kp = kponkd * kd
ki = kionkd * kd


% transfer function calculated by hand because I can't be bothered to
% properly learn matlab.

%{
% equations as derived by hand
% thetaddot equation
thetaddot_coeff = (coeff_mb2_l2) - coeff_Ib_mbl * coeff_mb_Iw_mw
thetaddot_torque_coeff = coeff_mb_l/r  + coeff_mb_Iw_mw
thetaddot_xdot_coeff = -b*coeff_mb_l/r
thetaddot_theta_coeff = -coeff_mb_l*g*coeff_mb_Iw_mw
thetaddot_add = coeff_mb2_l2;

% xddot equation
xddot_coeff = coeff_mb_Iw_mw*coeff_Ib_mbl - coeff_mb2_l2;
xddot_torque_coeff = coeff_mb_l + (coeff_Ib_mbl)/r;
xddot_xdot_coeff = -b*coeff_mb_l/(r^2);
xddot_theta_coeff = -coeff_mb2_l2*g;


num = [thetaddot_torque_coeff*xddot_coeff -thetaddot_torque_coeff*xddot_xdot_coeff+thetaddot_xdot_coeff*xddot_torque_coeff]
den = [thetaddot_coeff*xddot_coeff -thetaddot_coeff*xddot_xdot_coeff -xddot_coeff*thetaddot_theta_coeff -thetaddot_theta_coeff*xddot_xdot_coeff+xddot_torque_coeff]

w = tf(num, den)
sys = zpk(w)

sys.DisplayFormat='roots'

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

% kd > p3/K
disp(['Kd must be > p3/K=', num2str(p3/K)]);

kd = 0.04
kponkd=abs(p1+p2)
kionkd=abs(p1*p2)
kp = kponkd * kd
ki = kionkd * kd

% d > p3
% a > -k*p3*d
%{d = p3*2;
a = -p3*d/K;
controller = zpk([p1 p2], [sys.Z{1}(1), -d], a)

p = controller*sys
process = minreal(p)

process.DisplayFormat='roots'
%}

%rlocus(process)

%res = sim(['self_balance_pidcont.slx'])

%}