MC100/Simulink/init_model.m

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This file is part of the hoverboard-new-firmware-hack-FOC project
% Compared to previouse commutation method, this project implements
% FOC (Field Oriented Control) for BLDC motors with Hall sensors.
% The new control methods offers superior performanace
% compared to previous method featuring:
% >> reduced noise and vibrations
% >> smooth torque output
% >> improved motor efficiency -> lower energy consumption
%
% Author: Emanuel FERU
% Copyright � 2019-2021 Emanuel FERU <aerdronix@gmail.com>
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Clear workspace
close all
clear
clc

% Load model parameters
Ts                  = 5e-6;                         % [s] Model sampling time (1 MHz)
f_ctrl              = 20e3;                         % [Hz] Controller frequency = 1/Ts_ctrl (16 kHz)
Ts_ctrl             = 1/f_ctrl;                         % [s] Controller sampling time (~16 kHz)
speed_ctrl       =  20;     %delay for f_ctrl
i_pwm_count         = f_ctrl/4;
i_dead              = 10;
i_adc               = 10;
i_sample_before     = 10;
i_Udc               = 48;
i_half_pwm_count    = i_pwm_count;
i_sample_min        = (i_dead + i_adc + i_sample_before);
i_hall_count_max    = 1/Ts;

% Motor Paramaters
Rs = 0.0918;
Ld = 0.0009262;
Lq = 0.001024;
band_current = 3.14 * 2 * 4000;
% Kp = 0.0009262 * 3.14 * 8000 = 26
% Ki = 0.0918/0.0009262 = 99.11
% Kserial = 99.11 / 0.5816 = 170.42
% Current sample 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       = 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));

% Speed limitations
n_max               = 5000;             % [rpm] Maximum motor speed: [-5000, 5000]

% open loop speed -> voltage lookup table
res_rpm_openloop     = 100;
delta_rpm_openloop = 100; % delta rpm that when speed decreased, enter open loop
min_start_voltage_openloop = 10;
min_openVol = 10;
a_rpm_step = i_Udc / (n_max / res_rpm_openloop);
% 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        = (i_hall_count_max * a_mechAngle * (pi/180) * (30/pi));     % [-] Speed calculation coefficient (factors are due to conversions rpm <-> rad/s)
z_maxCntRst         = i_hall_count_max;   % [-] Maximum counter value for reset
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;