% Add path to zeroLoadCurrent() function
addpath('C:\Users\Andy\Documents\School\MicroCART\GitRepo\MicroCART_17-18\controls\MATLAB\zeroLoadCurrent')

% Import data as a table.
data = readtable('C:\Users\Andy\Documents\School\MicroCART\GitRepo\MicroCART_17-18\controls\dataCollection\Thrust and Drag Constant Data v2.xlsx');

filePath_noLoadCurrentData = 'C:\Users\Andy\Documents\School\MicroCART\GitRepo\MicroCART_17-18\controls\dataCollection\No Load Friction Current.xlsx';

[I_0, I_1, I_2] = zeroLoadCurrent(filePath_noLoadCurrentData);

rotor_speed_0 = data.(2) * (pi/30);
rotor_speed_1 = data.(3) * (pi/30);
rotor_speed_2 = data.(4) * (pi/30);
rotor_speed_3 = data.(5) * (pi/30);

% Define Pmin and Pmax, as well as the motor parameters Rm, Kv, Kq, and If
Pmin = 1e5;
Pmax = 2e5;
Rm = 0.2308;
Kv = 96.3422;
Kq = 96.3422;
If0 = I_0 * sign(rotor_speed_0) + I_1 * rotor_speed_0 + I_2 * rotor_speed_0.^2;
If1 = I_0 * sign(rotor_speed_1) + I_1 * rotor_speed_1 + I_2 * rotor_speed_1.^2;
If2 = I_0 * sign(rotor_speed_2) + I_1 * rotor_speed_2 + I_2 * rotor_speed_2.^2;
If3 = I_0 * sign(rotor_speed_3) + I_1 * rotor_speed_3 + I_2 * rotor_speed_3.^2;
If = [If0; If1; If2; If3];

% Call the calc_thrust_constant() function.
Kt = calcThrustConstant(data)

% Call the calc_drift_constant() function.
Kd = calcDragConstant(data, Pmin, Pmax, Rm, Kv, Kq, If)

