% Clear workspace
close all
clear
clc

% Load model parameters
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        = 500;                          % [Hz] Speed/torque Controller frequency = (500 Hz)
Ts_Spd_ctl          = 1/f_speed_ctrl;
n_spdctl_count      = f_ctrl/f_speed_ctrl;         % [count] Delay for f_speed_ctrl of the FOC controller
i_pwm_count         = 3000;
n_hall_count_ps     = 1/Ts;    % counts of per second
n_ramp_count        = f_ctrl/100; %10ms
%Current sample hw parameters
f_adc_curr_ceof = 0.0942;
f_lpf_idq  = 0.4; %Phase Current LowPass filter
f_lpf_vdq    = 0.01; %DC link current limit LowPass filter

pll_w = 100;
%Simulink provider Motor parameters
n_polePairs  = 4;        % [-] Number of motor pole pairs
PM           = 0.1688;   % Permanent magnet flux linkage, 
Ld           = 0.9262e-3;% d-axis inductance, 
Lq           = 1.024e-3; % q-axis inductance,
Rs           = 0.0918;   % Stator resistance,
J            = 0.03945; % Moment of inertia,
i_Udc        = 300;      % DCbus max voltage
i_dqMax      = 150;      % [A] Maximum allowed motor current (continuous)
n_max        = 2500;             % [rpm] Maximum motor speed: [-5000, 5000]
%BlueShark Motor parameters
%n_polePairs  = 5;           % [-] Number of motor pole pairs
%PM           = 0.0191471;   % Permanent magnet flux linkage, 
%Ld           = 0.08971e-3;  % d-axis inductance, 
%Lq           = 0.1128e-3;   % q-axis inductance,
%Rs           = 0.01587;     % Stator resistance,
%J            = 0.0014837*2;      % Moment of inertia,
%i_Udc        = 100;         % DCbus max voltage
%i_dqMax      = 270;         % [A] Maximum allowed motor current (continuous)
%n_max        = 5000;             % [rpm] Maximum motor speed: [-5000, 5000]
%Hall parameters
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

% 3 2 6 4 5 1 -> [7 5 1 0 3 4 2 7]
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       = 1;
a_elecAngle_XA      = 0:res_elecAngle:360;          % [deg] Electrical angle grid
r_sin_M1            = sin((a_elecAngle_XA)*(pi/180));  
r_cos_M1            = cos((a_elecAngle_XA)*(pi/180));

%% 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    = 310;              % [-] Error input target threshold (for "Blocked motor" detection)

% Speed Calculation Parameters
cf_speedCoef        = (n_hall_count_ps * a_elecDeltaAngle * (pi/180) * (30/pi));     % [-] Speed calculation coefficient (factors are due to conversions rpm <-> rad/s)
z_maxStillSecond     = 2; %(second, also as time-out to detect standing still)
n_commDeacvHi       = 30;               % [rpm] Commutation method deactivation speed high
n_commAcvLo         = 15;               % [rpm] Commutation method activation speed low
dz_cntTrnsDetHi     = 140;               % [-] Counter gradient High for transient behavior detection (used for speed estimation)
dz_cntTrnsDetLo     = 100;               % [-] Counter gradient Low for steady state detection (used for speed estimation)
n_stdStillDet       = 3;                % [rpm] Speed threshold for Stand still detection

n_SpeedModeLo       = 200; % min speed for exit speed ctrl mode
n_SpeedModeHi       = 300; % when speed is Hi can into speed ctrl mode

% Control model request
OPEN_MODE           = 0;                % [-] Open mode
SPD_MODE            = 1;                % [-] Speed mode
TRQ_MODE            = 2;                % [-] Torque mode

% open start vq ramp
dz_OpenStepVol     = 10;

% MTPA process
b_MTPA_Enable       = 0;                % MTPA enable flag: 0 = disable, 1 = enable
cf_MTPA_ldqdiff     = PM/(Lq - Ld);

% Field_Weakening
b_fieldWeakEna      = 1;                % [-] Field weakening enable flag: 0 = disable (default), 1 = enable
cf_Fw_Ki            = 0.5;
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
Vq_max              = i_Udc * V_modulation;
Vd_max              = Vq_max;
Qxis_D              = 1; %D轴优先
Qxis_Q              = 2; %Q轴优先
Qxis_F              = 3; %平均分配
% Current loop FeedForward 
b_FF_Enable = 0;
% D axis control gains
%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.0;
cf_iBand            = 2 * pi * f_ctrl / 20 * 3;

cf_idKp             = Ld * cf_iBand;                % [-] P gain
cf_idKi             = Rs/(Ld * f_ctrl) * cf_iGain;  % [-] I gain
cf_idKb             = cf_idKi;

% Q axis control gains
cf_iqKp             = Lq * cf_iBand;                % [-] P gain
cf_iqKi             = Rs/(Lq * f_ctrl) * cf_iGain;  % [-] I gain
cf_iqKb             = cf_iqKi;

% Speed control gains
cf_nKp              = 0.15;             % [-] P gain
cf_nKi              = 4.5 / f_speed_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

% Torque iq limit
cf_TrqLimKp              = 0.15;             % [-] P gain
cf_TrqLimKi              = 4.5 / f_speed_ctrl;    % [-] I gain
cf_TrqLimKb              = 1.0; %cf_TrqLimKi;