% Clear workspace
close all
clear
clc

% Load model parameters
Ts                  = 5e-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)
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_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;

%VBUS sample parameters

b_start_with_commutation = 0;
% Sine/Cosine wave look-up table
res_elecAngle       = 0.25;
a_elecAngle_XA      = 0:res_elecAngle:360;          % [deg] Electrical angle grid
a_elecAngle_XA        = fixpt_evenspace_cleanup(a_elecAngle_XA, sfix(16), 2^-3);   % 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;

% 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
% 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
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
b_fieldWeakEna      = 0;                % [-] Field weakening enable flag: 0 = disable (default), 1 = enable
r_fieldWeakHi       = 1000;             % [1000, 1500] Input target High threshold for reaching maximum Field Weakening / Phase Advance
r_fieldWeakLo       = 750;              % [ 500, 1000] Input target Low threshold for starting Field Weakening / Phase Advance
n_fieldWeakAuthHi   = 400;              % [rpm] Motor speed High for field weakening authorization
n_fieldWeakAuthLo   = 300;              % [rpm] Motor speed Low for field weakening authorization

% FOC method
id_fieldWeakMax     = 5;        % [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_XA           = 0:1:Vd_max;
Vq_max_M1           = sqrt(Vd_max^2 - Vq_max_XA.^2);  % Circle limitations look-up table
i_sca               = 1;                           % [-] [not tunable] Scalling factor A to int16 (50 = 1/0.02)
% Current Limitations
i_max               = 120;               % [A] Maximum allowed motor current (continuous)
i_max               = i_max * i_sca;
iq_maxSca_XA        = 0:0.02:0.99;
iq_maxSca_XA        = fixpt_evenspace_cleanup(iq_maxSca_XA, ufix(16), 2^-16);   % Make sure the data is evely spaced up to the last bit
iq_maxSca_M1        = sqrt(1 - iq_maxSca_XA.^2);                                % Current circle limitations map
  
% D axis control gains
cf_idKp             = 4.0;              % [-] P gain
cf_idKi             = 0.05;  % [-] I gain
cf_idKb             = 1.0;
% Q axis control gains
cf_iqKp             = 4.0;              % [-] P gain
cf_iqKi             = 0.05; % [-] I gain
cf_iqKb             = 1.0;

% Speed control gains
cf_nKp              = 3.1;             % [-] P gain
cf_nKi              = 0.01;% [-] I gain
cf_nKb              = 0.02;

% Torque iq limit
cf_torqKLimHi = 0.8;
cf_torqKLimLo =  -0.1;