% 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        = 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               = 300;
i_half_pwm_count    = i_pwm_count;
n_hall_count_ps    = 1/Ts;    % counts of per second

%Current sample hw parameters
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       = 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_mechAngle * (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
z_ctrlModReq        = TRQ_MODE;         % [-] Control Mode Request (default)

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

% 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
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               = 200;               % [A] Maximum allowed motor current (continuous)

% 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;
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             = 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              = 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

% Torque iq limit
cf_TrqLimKp              = 0.3;             % [-] P gain
cf_TrqLimKi              = 0.02;    % [-] I gain
cf_TrqLimKb              = 0.1;