% Read in data 
data = readtable('C:\Users\Andy\Documents\MATLAB\MicroCART\Thrust and Drag Constant\Thrust and Drag Constant Data.xlsx');

filePath_noLoadCurrentData = 'C:\Users\Andy\Documents\MATLAB\MicroCART\Zero Load Current\No Load Friction Current.csv';

[I_0, I_1, I_2, dutyCycle_error, error, residual_error, residual_error_ConstantIf] = zeroLoadCurrent(filePath_noLoadCurrentData);

Pmin = 0.40;
Pmax = 0.8;
Rm = 0.2308;
Kv = 96.3422;
Kq = 96.3422;

% Convert RPM to angular speed of each rotor.
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);

duty_cycle_percentage = data.(1);
Vb = data.(6);

u = ((data.(1)/100) - Pmin)/(Pmax - Pmin);

If = I_0 * sign(rotor_speed_0) + I_1 * rotor_speed_0 + I_2 * rotor_speed_0.^2;
w_num = -1 + sqrt( 1 - 4*Rm*Kv*Kq*Kd*(Kv*Rm*If - Kv*u.*Vb));
w_den = 2*Rm*Kv*Kq*Kd;
w = w_num / w_den;

figure()
hold on
plot(duty_cycle_percentage, w); grid()
plot(duty_cycle_percentage, rotor_speed_0);
%plot(duty_cycle_percentage, rotor_speed_1);
%plot(duty_cycle_percentage, rotor_speed_2);
%plot(duty_cycle_percentage, rotor_speed_3);

xlabel('Duty Cycle Percentage');
ylabel('Rotor Speed (rad/s)')

residual_error

residual_error_ConstantIf