diff --git a/MATLAB/thrustAndDragConstant/calc_drag_constant.m b/MATLAB/thrustAndDragConstant/calc_drag_constant.m new file mode 100644 index 0000000000000000000000000000000000000000..53a7cc6b4ccef432358e58831223b90ad6125e7c --- /dev/null +++ b/MATLAB/thrustAndDragConstant/calc_drag_constant.m @@ -0,0 +1,51 @@ +function [ Kd ] = calc_drag_constant( data, Pmin, Pmax, Rm, Kv, Kq, If ) +%CALC_DRAG_CONSTANT +% Calculates the drag constant (Kd) given experimental data. The drag +% constant is described in detail in sections 4.2.5, 4.2.6, and 5.5.4.1 +% of "Model development, system identification, and control of a +% quadrotor helicopter" by Matt Rich. + +% Input Arguments: +% data: experimental data +% Pmax: Calculated maximum duty cycle of the function generators PWM wave +% that the ESC can handle. +% Pmin: Calculated minimum duty cycle of the function generators PWM wave +% for initialization of the ESC. +% Rm: Motor resistance +% Kv: Back-EMF constant of the motor +% Kq: Torque constant of the motor +% If: No-Load (friction) current of the motor + + +% 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); + +% Refer to the sections described in the header of this function +% for a better understanding of what this loop is doing. Note that these +% equations for each individual rotor. Calculating the total drag constant +% requires taking into account all four motors. In this function this is +% done by stacking the data of each motor in a single array and performing +% a least squares approximation of all of the data. +% +% u: Defined in section 4.2.5 +% omega: vector of each rotors speed in rad/s +% A: column vector of each rotors speed squared +% b: Defined in section 5.5.4.1 +% Kd_vector: Vector containing all experimental Kd values +% Vb: Battery voltage +u1 = ((data.(1)/100) - Pmin)/(Pmax - Pmin); +u2 = ((data.(1)/100) - Pmin)/(Pmax - Pmin); +u3 = ((data.(1)/100) - Pmin)/(Pmax - Pmin); +u4 = ((data.(1)/100) - Pmin)/(Pmax - Pmin); +u = [u1; u2; u3; u4]; +Vb = [data.(6); data.(6); data.(6); data.(6)]; +omega = [rotor_speed_0; rotor_speed_1; rotor_speed_2; rotor_speed_3]; +A = omega.^2; +b = ((u.*Vb)/(Rm*Kq))-omega./(Rm*Kq*Kv)-If/Kq; + +Kd = ((A'*A)^-1)*A'*b +end + diff --git a/MATLAB/thrustAndDragConstant/calc_thrust_constant.m b/MATLAB/thrustAndDragConstant/calc_thrust_constant.m new file mode 100644 index 0000000000000000000000000000000000000000..202b41eaafa54eec47d1237e9febcb7dca3db5fa --- /dev/null +++ b/MATLAB/thrustAndDragConstant/calc_thrust_constant.m @@ -0,0 +1,31 @@ +function [ Kt ] = calc_thrust_constant( data ) +%CALC_THRUST_CONSTANT +% Calculates the thrust constant (Kt) given experimental data. The thrust +% constant is described in detail in sections 4.2.1.1 and 5.5.1 of +% "Model development, system identification, and control of a quadrotor +% helicopter" by Matt Rich. + +% Input Arguments: +% data: experimental data + +% 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); + +% Define the A matrix as the sum of each rotors speed squared. +A = (rotor_speed_0.^2 + rotor_speed_1.^2 + rotor_speed_2.^2 + rotor_speed_3.^2); + +% Convert weight (g) to thrust force (N) by converting grams to kilograms and +% multiplying by the acceleration of gravity. +T = (data.Scale_g_/1000)*9.8; + +% Calculate the thrust constant (Kt) through the following +% equation: Kt = (A'A)^-1.*A'.*T as defined on page 65 of "Model +% development, system identification, and control of a quadrotor +% helicopter" (Matt Rich's Thesis). +Kt = ((A'*A)^(-1))*(A'*T) + +end + diff --git a/MATLAB/thrustAndDragConstant/thrust_drag_constant_calculations.m b/MATLAB/thrustAndDragConstant/thrust_drag_constant_calculations.m new file mode 100644 index 0000000000000000000000000000000000000000..1f8caf84b5df55c3bff5f34dd0bcab6f1ad14b66 --- /dev/null +++ b/MATLAB/thrustAndDragConstant/thrust_drag_constant_calculations.m @@ -0,0 +1,30 @@ +% Import data as a table. +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] = zero_load_current(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 = 0.40; +Pmax = 0.8; +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 = calc_thrust_constant(data); + +% Call the calc_drift_constant() function. +Kd = calc_drag_constant(data, Pmin, Pmax, Rm, Kv, Kq, If ); + diff --git a/MATLAB/zeroLoadCurrent/Kd_error.m b/MATLAB/zeroLoadCurrent/Kd_error.m new file mode 100644 index 0000000000000000000000000000000000000000..8ecec2bdca8036db3f4c3e0e651a436d94ad5585 --- /dev/null +++ b/MATLAB/zeroLoadCurrent/Kd_error.m @@ -0,0 +1,48 @@ +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_withIfConstant] = zero_load_current(filePath_noLoadCurrentData); + +Pmin = 0.40; +Pmax = 0.8; +Rm = 0.2308; +Kv = 96.3422; +Kq = 96.3422; +If = 0.44; + +% 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)') + +figure() +plot(dutyCycle_error, error); grid() +xlabel('Duty Cycle Percentage'); +ylabel('Error'); + +residual_error + +residual_error_withIfConstant \ No newline at end of file diff --git a/MATLAB/zeroLoadCurrent/zero_load_current.m b/MATLAB/zeroLoadCurrent/zero_load_current.m new file mode 100644 index 0000000000000000000000000000000000000000..a9ec2e607b5bfcfa866c0000f4f296b5af103104 --- /dev/null +++ b/MATLAB/zeroLoadCurrent/zero_load_current.m @@ -0,0 +1,61 @@ +function [I_0, I_1, I_2, dutyCycle, error, residual_error, residual_error_withIfConstant] = zero_load_current(absoluteFilePath) +%This function takes in an absolute file path to a .csv or Excel file with +%the following format: +% Second column should be the motor speed in revolutions per minute +% Third column should be the current (A) +%absoluteFilePath = 'C:\Users\Andy\Documents\MATLAB\MicroCART\Zero Load Current\No Load Friction Current.csv'; + + %Get the data from the file + data = readtable(absoluteFilePath); + + %Extract the motor speed column and convert it to radians per second + dutyCycle = data.(1); + motorSpeed_rpm = data.(2); + motorSpeed_radPerSec = motorSpeed_rpm * pi/30; + + %Extract the current column + current = data.(3); + + %Define the three columns of the A matrix for the least squares + %approximation, using this equation: + % + % I_f = sgn(motorSpeed)*I_0 + motorSpeed*I_1 + (motorSpeed^2)*I_2 + % + % for this least squares equation: A*x = b (where A and b are matrices) + % + % where: sgn(motorSpeed) -- first column of the A matrix, + % motorSpeed -- second column of the A matrix + % motorSpeed^2 -- third column of the A matrix + % I_f -- b matrix + % + % and: [I_0; I_1; I_2] -- x vector that we are solving for + A_col1 = sign(motorSpeed_radPerSec); + A_col2 = motorSpeed_radPerSec; + A_col3 = motorSpeed_radPerSec .^ 2; + + %Create the A matrix from its three columns + A = [A_col1, A_col2, A_col3]; + + %Define the b matrix + b = current; + + %Least squares approximation -- solving for x, which is I_vector + I_vector = (A' * A)^(-1) * A' * b; + + %Get the error vector + error = A*I_vector - b; + + If_constant = 0.511; + error_withIfConstant = A*[If_constant; 0; 0] - b; + + %Get the residual (?) + residual_error = sum(error.^2); + + residual_error_withIfConstant = sum(error_withIfConstant.^2); + + %Extract the components of the vector + I_0 = I_vector(1); + I_1 = I_vector(2); + I_2 = I_vector(3); + +end %function