% Physical Constant
g = 9.81;                       % gravity acceleration重力加速度 [m/sec^2]

% NXTway-GS Parameters平衡车相关参数
m = 0.015;                      % wheel weight车轮重量(个人更改) [kg]
R = 0.04;                       % wheel radius车轮半径(个人更改) [m]
Jw = m * R^2 / 2;               % wheel inertia moment车轮惯性力矩 [kgm^2]
M = 1.5;                        % body weight车体重量(个人更改) [kg]
W = 0.23;                       % body width车体宽度(个人更改) [m]
D = 0.12;                       % body depth车体深度(个人更改) [m]
H = 0.17;                       % body height车体高度(个人更改) [m]
L = 0.075;%H / 2;               % distance of the center of mass from the wheel axle [m]
                                % 车体重心高度，一般为车体高度一半(个人更改)
Jpsi = M * L^2 / 3;             % body pitch inertia moment车体惯性力矩(取重心高度) [kgm^2]
Jphi = M * (W^2 + D^2) / 12;    % body yaw inertia moment车体偏航惯性力矩 [kgm^2]
fm = 0.0022;                    % friction coefficient between body & DC motor
                                % 车体与电机之间的摩擦系数
fw = 0;                         % friction coefficient between wheel & floor
                                % 轮胎与地面之间的摩擦系数
% DC Motor Parameters电机相关参数         
Jm = 5.7e-7;                    % DC motor inertia moment电机惯性力矩(基本可以忽略) [kgm^2]
Rm = 2.7;                       % DC motor resistance电机阻抗 [ohm]
Kb = 0.013;                     % DC motor back EMF constant电机反电动势 [Vsec/rad]
Kt = 0.013;                     % DC motor torque constant电机转矩常数 [Nm/A]
n = 64;                         % gear ratio齿轮比，减速比

% NXTway-GS State-Space Matrix Calculation矩阵计算
alpha = n * Kt / Rm;
beta = n * Kt * n* Kb / Rm + fm;
tmp = beta + fw;

E_11 = (2 * m + M) * R^2 + 2 * Jw + 2 * n^2 * Jm;
E_12 = M * L * R - 2 * n^2 * Jm;
E_22 = M * L^2 + Jpsi + 2 * n^2 * Jm;
detE = E_11 * E_22 - E_12^2;

A1_32 = -g * M * L * E_12 / detE;
A1_42 = g * M * L * E_11 / detE;
A1_33 = -2 * (tmp * E_22 + beta * E_12) / detE;
A1_43 = 2 * (tmp * E_12 + beta * E_11) / detE;
A1_34 = 2 * beta * (E_22 + E_12) / detE;
A1_44 = -2 * beta * (E_11 + E_12) / detE;
B1_3 = alpha * (E_22 + E_12) / detE;
B1_4 = -alpha * (E_11 + E_12) / detE;
A1 = [
    0 0 1 0
    0 0 0 1
    0 A1_32 A1_33 A1_34
    0 A1_42 A1_43 A1_44
    ];



B1 = [
    0 0
    0 0
    B1_3 B1_3
    B1_4 B1_4
    ];
C1 = eye(4);
D1 = zeros(4, 2);

I = m * W^2 / 2 + Jphi + (Jw + n^2 * Jm) * W^2 / (2 * R^2);
J = tmp * W^2 / (2 * R^2);
K = alpha * W / (2 * R);
A2 = [
    0 1
    0 -J / I
    ];
B2 = [
    0      0
    -K / I K / I
    ];
C2 = eye(2);
D2 = zeros(2);

clear alpha beta tmp
clear E_11 E_12 E_22 detE
clear A1_32 A1_33 A1_34 A1_42 A1_43 A1_44 B1_3 B1_4 I J K

% Controller Parameters 

% Servo Gain Calculation using Optimal Regulator
A_BAR = [A1, zeros(4, 1); C1(1, :), 0];
B_BAR = [B1; 0, 0];
QQ = [
    1, 0,   0, 0, 0
    0, 6e5, 0, 0, 0
    0, 0,   1, 0, 0
    0, 0,   0, 1, 0
    0, 0,   0, 0, 4e2
    ];
RR = 1e3 * eye(2);
KK = lqr(A_BAR, B_BAR, QQ, RR);
k_f = KK(1, 1:4);                   % feedback gain
k_i = KK(1, 5);                     % integral gain

ts1 = 0.002;
a_r = 0.996;                        % smooth reference signal
dt = 0.005


% suppress velocity gain because it fluctuates NXTway-GS
% k_f(3) = k_f(3) * 0.85;