% Controller Parameters
% Servo Gain Calculation using Optimal Regulator
A_BAR = [A, zeros(4, 1); C(1, :), 0];
B_BAR = [B; 0];
Q = [
1, 0, 0, 0, 0
0, 6e5, 0, 0, 0
0, 0, 1, 0, 0
0, 0, 0, 1, 0
0, 0, 0, 0, 1e3
];
R = 6e3 * (180 / pi)^2; % R = 6e3 if the unit of angles is [rad].
K = lqr(A_BAR, B_BAR, Q, R);
k_f = K(1:4); % feedback gain
k_i = K(5); % integral gain
% Task Sample Rates
ts1 = 4; % ts1 sample time [msec]
ts2 = 20; % ts2 sample time [msec]
ts3 = 100; % ts3 sample time [msec]
% Time Variables
time_start = 1000; % start time of balancing [msec] (= gyro calibration time)
time_auto = 5000; % start time of autonomous drive [msec]
time_stop = 500; % duration time of out of control [msec]
% Operating State Enumeration
calib = 1; % gyro calibration
control = 2; % balance and drive control
stop = 3; % stop due to out of control
% Parameters of Coulombic & Viscous Friction
pwm_gain = 1; % pwm gain
pwm_offset = 0; % pwm offset
% Low Path Filter Coefficients
a_b = 0.8; % average battery value
a_d = 0.8; % suppress velocity noise
a_r = 0.996; % smooth reference signal
a_gc = 0.8; % calibrate gyro offset
a_gd = 0.999; % compensate gyro drift
% User Setting Values
k_thetadot = 0.12 / Rs * 180 / pi; % speed gain (0.12 [m/sec])
dst_thr = 30; % distance threshold of obstacle avoidance [cm]
psi_diff_thr = 45; % body pitch angle threshold [deg]
theta_diff_thr = 4 * 360; % ball rolling angle threshold [deg]
sound_frq = 440; % sound frequency [Hz]
sound_dur = 500; % sound duration [msec]
gp_max = 100; % maximum game pad input
log_count = 25; % data logging count (logging sample time = ts1 * log_count)
lsb = 2^-10; % data logging LSB
clear A_BAR B_BAR Q R K
