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