|
|
@@ -4,13 +4,13 @@ clear
|
|
|
clc
|
|
|
|
|
|
% Load model parameters
|
|
|
-Ts = 5e-6; % [s] Model sampling time (200 KHz)
|
|
|
+Ts = 1e-6; % [s] Model sampling time (200 KHz)
|
|
|
f_ctrl = 20e3; % [Hz] Controller frequency = 1/Ts_ctrl (20 kHz)
|
|
|
Ts_ctrl = 1/f_ctrl; % [s] Controller sampling time (50us)5e-5
|
|
|
-f_speed_ctrl = 1e3; % [Hz] Speed/torque Controller frequency = (1 kHz)
|
|
|
+f_speed_ctrl = 5e2; % [Hz] Speed/torque Controller frequency = (1 kHz)
|
|
|
speed_ctrl = f_ctrl/f_speed_ctrl; % [count] Delay for f_speed_ctrl of the FOC controller
|
|
|
i_pwm_count = 3000;
|
|
|
-i_Udc = 48;
|
|
|
+i_Udc = 300;
|
|
|
i_half_pwm_count = i_pwm_count;
|
|
|
n_hall_count_ps = 1/Ts; % counts of per second
|
|
|
|
|
|
@@ -19,36 +19,36 @@ n_adc_max = 4096;
|
|
|
n_resistance = 0.0005;
|
|
|
n_ref_vol = 3.3;
|
|
|
n_gain = 17.1;
|
|
|
+f_lpf_coeff = 0.4; %Phase Current LowPass filter
|
|
|
+
|
|
|
+% Motor parameters
|
|
|
+n_polePairs = 4; % [-] Number of motor pole pairs
|
|
|
+PM = 0.1688; %0.03; % Permanent magnet flux linkage,
|
|
|
+Ld = 0.9262e-3;%2e-4; % d-axis inductance,
|
|
|
+Lq = 1.024e-3; %2e-4; % q-axis inductance,
|
|
|
+Rs = 0.0918; %0.013; % Stator resistance,
|
|
|
+J = 0.03945; %0.2; % Moment of inertia,
|
|
|
+
|
|
|
+%Hall parameters
|
|
|
+n_max = 8000; % [rpm] Maximum motor speed: [-5000, 5000]
|
|
|
+a_elecPeriod = 360; % [deg] Electrical angle period
|
|
|
+a_elecDeltaAngle = 60; % [deg] Electrical angle between two Hall sensor changing events
|
|
|
+a_mechAngle = a_elecDeltaAngle / n_polePairs; % [deg] Mechanical angle between two Hall sensor changing events
|
|
|
+
|
|
|
+vec_hallToPos = [7 5 1 0 3 4 2 7]; % [-] Mapping Hall signal to position
|
|
|
+i_hall_offset = -30; %60;%-30;
|
|
|
+b_angleMeasEna = 0;
|
|
|
|
|
|
% Sine/Cosine wave look-up table
|
|
|
-res_elecAngle = 0.25;
|
|
|
+res_elecAngle = 1;
|
|
|
a_elecAngle_XA = 0:res_elecAngle:360; % [deg] Electrical angle grid
|
|
|
-a_elecAngle_XA = fixpt_evenspace_cleanup(a_elecAngle_XA, sfix(16), 2^-4); % Make sure the data is evely spaced up to the last bit
|
|
|
r_sin_M1 = sin((a_elecAngle_XA)*(pi/180));
|
|
|
r_cos_M1 = cos((a_elecAngle_XA)*(pi/180));
|
|
|
|
|
|
-% Speed limitations
|
|
|
-n_max = 5000; % [rpm] Maximum motor speed: [-5000, 5000]
|
|
|
-
|
|
|
-% open loop speed -> voltage lookup table
|
|
|
-min_openVol = 10;
|
|
|
-
|
|
|
-% Motor parameters
|
|
|
-n_polePairs = 4; % [-] Number of motor pole pairs
|
|
|
-a_elecPeriod = 360; % [deg] Electrical angle period
|
|
|
-a_elecDeltaAngle = 60; % [deg] Electrical angle between two Hall sensor changing events
|
|
|
-a_mechAngle = a_elecDeltaAngle / n_polePairs; % [deg] Mechanical angle between two Hall sensor changing events
|
|
|
-r_whl = 6.5 * 2.54 * 1e-2 / 2; % [m] Wheel radius. Diameter = 6.5 inch (1 inch = 2.54 cm): Speed[kph] = rpm*(pi/30)*r_whl*3.6
|
|
|
-
|
|
|
-f_lpf_coeff = 0.4;
|
|
|
%% F02_Diagnostics
|
|
|
t_errQual = 0.24 * f_ctrl/3; % [s] Error qualification time
|
|
|
t_errDequal = 1.80 * f_ctrl/3; % [s] Error dequalification time
|
|
|
-r_errInpTgtThres = 15; % [-] Error input target threshold (for "Blocked motor" detection)
|
|
|
-
|
|
|
-%hall, [4,6,2,3,1,5,4] [ 3,2,6,4,5,1]
|
|
|
-vec_hallToPos = [7 5 1 0 3 4 2 7]; % [-] Mapping Hall signal to position
|
|
|
-i_hall_offset = 60;%-30;
|
|
|
+r_errInpTgtThres = 310; % [-] Error input target threshold (for "Blocked motor" detection)
|
|
|
|
|
|
% Speed Calculation Parameters
|
|
|
cf_speedCoef = (n_hall_count_ps * a_mechAngle * (pi/180) * (30/pi)); % [-] Speed calculation coefficient (factors are due to conversions rpm <-> rad/s)
|
|
|
@@ -61,8 +61,6 @@ n_stdStillDet = 3; % [rpm] Speed threshold for Stand still
|
|
|
|
|
|
n_SpeedModeLo = 200; % min speed for exit speed ctrl mode
|
|
|
n_SpeedModeHi = 300; % when speed is Hi can into speed ctrl mode
|
|
|
-% Motor Angle Measurement (e.g. using an encoder)
|
|
|
-b_angleMeasEna = 0; % [-] Enable flag for external mechanical motor angle sensor: 0 = estimated (default), 1 = measured
|
|
|
|
|
|
% Control model request
|
|
|
OPEN_MODE = 0; % [-] Open mode
|
|
|
@@ -70,46 +68,52 @@ SPD_MODE = 1; % [-] Speed mode
|
|
|
TRQ_MODE = 2; % [-] Torque mode
|
|
|
z_ctrlModReq = TRQ_MODE; % [-] Control Mode Request (default)
|
|
|
|
|
|
-% Cruise control
|
|
|
-b_cruiseCtrlEna = 0; % [-] Cruise control enable flag: 0 = disable (default), 1 = enable
|
|
|
-n_cruiseMotTgt = 0; % [-] Cruise control motor speed target
|
|
|
-
|
|
|
-%% F04_Field_Weakening
|
|
|
+% MTPA process
|
|
|
b_MTPA_Enable = 0; % MTPA enable flag: 0 = disable, 1 = enable
|
|
|
-b_fieldWeakEna = 0; % [-] Field weakening enable flag: 0 = disable (default), 1 = enable
|
|
|
-cf_Fw_Ki = 0.0002;
|
|
|
-cf_Fw_Atw = 0.01;
|
|
|
+cf_MTPA_ldqdiff = PM/(Lq - Ld);
|
|
|
|
|
|
-% FOC method
|
|
|
-id_fieldWeakMax = -30; % [A] Field weakening maximum current
|
|
|
+% Field_Weakening
|
|
|
+b_fieldWeakEna = 0; % [-] Field weakening enable flag: 0 = disable (default), 1 = enable
|
|
|
+cf_Fw_Ki = 2;
|
|
|
+cf_Fw_Kb = 0.1;
|
|
|
+id_fieldWeakMax = -50; % [A] Field weakening maximum current
|
|
|
|
|
|
% Voltage Limitations
|
|
|
V_modulation = 0.95; % [-] Voltage margin to make sure that there is a sufficiently wide pulse for a good phase current measurement
|
|
|
-Vd_max = i_Udc * V_modulation;
|
|
|
+Vq_max = i_Udc * V_modulation;
|
|
|
+Vd_max = Vq_max;
|
|
|
Vq_max_XA = 0:1:Vd_max;
|
|
|
+%Vq_max_XA = fixpt_evenspace_cleanup(Vq_max_XA, ufix(16), 2^-5);
|
|
|
Vq_max_M1 = sqrt(Vd_max^2 - Vq_max_XA.^2); % Circle limitations look-up table
|
|
|
-
|
|
|
% Current Limitations
|
|
|
-i_dqMax = 120; % [A] Maximum allowed motor current (continuous)
|
|
|
+i_dqMax = 200; % [A] Maximum allowed motor current (continuous)
|
|
|
|
|
|
+% Current loop FeedForward
|
|
|
+b_FF_Enable = 0;
|
|
|
% D axis control gains
|
|
|
-cf_idKp = 4.0; % [-] P gain
|
|
|
-cf_idKi = 0.05; % [-] I gain
|
|
|
-cf_idKb = 1.0;
|
|
|
+%https://www.it610.com/article/1282852898983657472.htm
|
|
|
+%https://e2echina.ti.com/support/microcontrollers/c2000/f/c2000-microcontrollers-forum/109265/lab05a-fast-pid
|
|
|
+cf_iGain = 1;
|
|
|
+cf_iBand = 2 * pi * f_ctrl / 20 * 1.7;
|
|
|
+
|
|
|
+cf_idKp = Ld * cf_iBand; %/ (1 * Ts_ctrl); % [-] P gain
|
|
|
+cf_idKi = Rs/(Ld * f_ctrl) * cf_iGain;%%%0.05; % [-] I gain
|
|
|
+cf_idKb = 0.1;
|
|
|
+
|
|
|
% Q axis control gains
|
|
|
-cf_iqKp = 2; % [-] P gain
|
|
|
-cf_iqKi = 0.025; % [-] I gain
|
|
|
-cf_iqKb = 1.0;
|
|
|
+cf_iqKp = Lq * cf_iBand; %/ (1.5 * Ts_ctrl); % [-] P gain
|
|
|
+cf_iqKi = Rs/(Lq * f_ctrl) * cf_iGain; % [-] I gain
|
|
|
+cf_iqKb = 0.1;
|
|
|
|
|
|
% Speed control gains
|
|
|
-cf_nKp = 3.1; % [-] P gain
|
|
|
-cf_nKi = 0.01;% [-] I gain
|
|
|
-cf_nKb = 0.05;
|
|
|
-cf_lastIqGain = -0.5; % when swtich toqure to speed mode, use the last iq as the speed PI init value
|
|
|
+cf_nKp = 0.2; % [-] P gain
|
|
|
+cf_nKi = 200 * Ts_ctrl; % [-] I gain
|
|
|
+cf_nKb = cf_nKi;
|
|
|
+cf_lastIqGain = 0.5; % when swtich toqure to speed mode, use the last iq as the speed PI init value
|
|
|
|
|
|
% DC-Link current limit, I controller
|
|
|
-cf_dcKi = 0.01;
|
|
|
-cf_dcKaw = 1.001;
|
|
|
+%cf_dcKi = 0.01;
|
|
|
+%cf_dcKaw = 1.001;
|
|
|
% Torque iq limit
|
|
|
-cf_torqKLimHi = 6.0;
|
|
|
-cf_torqKLimLo = 0.1;
|
|
|
+%cf_torqKLimHi = 6.0;
|
|
|
+%cf_torqKLimLo = 0.1;
|
huhui