-
Notifications
You must be signed in to change notification settings - Fork 95
/
init_model.m
284 lines (239 loc) · 13 KB
/
init_model.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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
load BLDCmotorControl_data;
Ts = 5e-6; % [s] Model sampling time (200 kHz)
Ts_ctrl = 6e-5; % [s] Controller sampling time (~16 kHz)
f_ctrl = 16e3; % [Hz] Controller frequency = 1/Ts_ctrl (16 kHz)
% Ts_ctrl = 12e-5; % [s] Controller sampling time (~8 kHz)
% Motor parameters
n_polePairs = 15; % [-] Number of motor pole pairs
a_elecPeriod = 360; % [deg] Electrical angle period
a_elecAngle = 60; % [deg] Electrical angle between two Hall sensor changing events
a_mechAngle = a_elecAngle / 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
i_sca = 50; % [-] [not tunable] Scalling factor A to int16 (50 = 1/0.02)
% Sine/Cosine wave look-up table
res_elecAngle = 2;
a_elecAngle_XA = 0:res_elecAngle:360; % [deg] Electrical angle grid
r_sin_M1 = sin((a_elecAngle_XA + 30)*(pi/180)); % Note: 30 deg shift is to allign it with the Hall sensors position
r_cos_M1 = cos((a_elecAngle_XA + 30)*(pi/180));
% figure
% stairs(a_elecAngle_XA, r_sin_M1); hold on
% stairs(a_elecAngle_XA, r_cos_M1);
% legend('sin','cos');
%% Control selection
% Control type selection
CTRL_COM = 0; % [-] Commutation Control
CTRL_SIN = 1; % [-] Sinusoidal Control
CTRL_FOC = 2; % [-] Field Oriented Control (FOC)
z_ctrlTypSel = CTRL_FOC; % [-] Control Type Selection (default)
% Control model request
OPEN_MODE = 0; % [-] Open mode
VLT_MODE = 1; % [-] Voltage mode
SPD_MODE = 2; % [-] Speed mode
TRQ_MODE = 3; % [-] Torque mode
z_ctrlModReq = VLT_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
%% F01_Estimations
% Position Estimation Parameters
% Hall = 4*hA + 2*hB + hC
% Hall = [0 1 2 3 4 5 6 7]
vec_hallToPos = [0 2 0 1 4 3 5 0]; % [-] Mapping Hall signal to position
% Speed Calculation Parameters
cf_speedCoef = round(f_ctrl * a_mechAngle * (pi/180) * (30/pi)); % [-] Speed calculation coefficient (factors are due to conversions rpm <-> rad/s)
z_maxCntRst = 2000; % [-] Maximum counter value for reset (works 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 = 40; % [-] Counter gradient High for transient behavior detection (used for speed estimation)
dz_cntTrnsDetLo = 20; % [-] Counter gradient Low for steady state detection (used for speed estimation)
n_stdStillDet = 3; % [rpm] Speed threshold for Stand still detection
cf_currFilt = 0.12; % [%] Current filter coefficient [0, 1]. Lower values mean softer filter
% Motor Angle Measurement (e.g. using an encoder)
b_angleMeasEna = 0; % [-] Enable flag for external mechanical motor angle sensor: 0 = estimated (default), 1 = measured
%% F02_Diagnostics
b_diagEna = 1; % [-] Diagnostics enable flag: 0 = Disabled, 1 = Enabled (default)
t_errQual = 0.24 * f_ctrl/3; % [s] Error qualification time
t_errDequal = 1.80 * f_ctrl/3; % [s] Error dequalification time
r_errInpTgtThres = 600; % [-] Error input target threshold (for "Blocked motor" detection)
%% F03_Control_Mode_Manager
dV_openRate = 1000 / (f_ctrl/3);% [V/s] Rate for voltage cut-off in Open Mode (Sample Time included in the rate)
%% 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 * i_sca; % [A] Field weakening maximum current
% SIN method
a_phaAdvMax = 25; % [deg] Maximum phase advance angle
%% F05_Field_Oriented_Control
z_selPhaCurMeasABC = 0; % [-] Select measured current phases: {iA,iB} = 0; {iB,iC} = 1; {iA,iC} = 2
% Motor Limitations Calibratables
cf_iqKiLimProt = 60 / (f_ctrl/3); % [-] Current limit protection integral gain (only used in VLT_MODE and SPD_MODE)
cf_nKiLimProt = 20 / (f_ctrl/3); % [-] Speed limit protection integral gain (only used in VLT_MODE and TRQ_MODE)
cf_KbLimProt = 1000 / (f_ctrl/3);% [-] Back calculation gain for integral anti-windup
% Voltage Limitations
V_margin = 100; % [-] Voltage margin to make sure that there is a sufficiently wide pulse for a good phase current measurement
Vd_max = 1000 - V_margin;
Vq_max_XA = 0:20:Vd_max;
Vq_max_M1 = sqrt(Vd_max^2 - Vq_max_XA.^2); % Circle limitations look-up table
% figure
% stairs(Vq_max_XA, Vq_max_M1); legend('V_{max}');
% Speed limitations
n_max = 1000; % [rpm] Maximum motor speed: [-1500, 1500]
% Current Limitations
i_max = 15; % [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
% figure
% stairs(iq_maxSca_XA, iq_maxSca_M1); legend('i_{maxSca}');
%-------------------------------
% Q axis control gains
cf_iqKp = 0.3; % [-] P gain
cf_iqKi = 100 / (f_ctrl/3); % [-] I gain
% D axis control gains
cf_idKp = 0.2; % [-] P gain
cf_idKi = 60 / (f_ctrl/3); % [-] I gain
% Speed control gains
cf_nKp = 1.18; % [-] P gain
cf_nKi = 20.4 / (f_ctrl/3);% [-] I gain
%-------------------------------
%% F06_Control_Type_Management
% Commutation method
z_commutMap_M1 = [-1 -1 0 1 1 0; % Phase A
1 0 -1 -1 0 1; % Phase B
0 1 1 0 -1 -1]; % Phase C [-] Commutation method map
omega = a_elecAngle_XA*(pi/180);
pha_adv = 30; % [deg] Phase advance to mach commands with the Hall position
r_sinPhaA_M1 = -sin(omega + pha_adv*(pi/180));
r_sinPhaB_M1 = -sin(omega - 120*(pi/180) + pha_adv*(pi/180));
r_sinPhaC_M1 = -sin(omega + 120*(pi/180) + pha_adv*(pi/180));
% Sinusoidal 3rd harmonic method
A = 1.15; % Sine amplitude (tunable to get the Saddle sin maximum to value 1000)
sin3Arm = -0.224*sin(3*(omega + pha_adv*(pi/180))); % 3rd armonic
r_sin3PhaA_M1 = sin3Arm + A*r_sinPhaA_M1;
r_sin3PhaB_M1 = sin3Arm + A*r_sinPhaB_M1;
r_sin3PhaC_M1 = sin3Arm + A*r_sinPhaC_M1;
disp('---- BLDC_controller: Initialization OK ----');
%% Plot control methods
show_fig = 0;
if show_fig
% Apply scaling
sca_factor = 1000; % [-] scalling factor (to avoid truncation approximations on integer data type)
r_sinPhaA_M1sca = sca_factor * r_sinPhaA_M1;
r_sinPhaB_M1sca = sca_factor * r_sinPhaB_M1;
r_sinPhaC_M1sca = sca_factor * r_sinPhaC_M1;
r_sin3PhaA_M1sca = sca_factor * r_sin3PhaA_M1;
r_sin3PhaB_M1sca = sca_factor * r_sin3PhaB_M1;
r_sin3PhaC_M1sca = sca_factor * r_sin3PhaC_M1;
% Commutation method
a_commElecAngle_XA = [0 60 120 180 240 300 360]; % [deg] Electrical angle grid
hall_A = [0 0 0 1 1 1 1] + 4;
hall_B = [1 1 0 0 0 1 1] + 2;
hall_C = [0 1 1 1 0 0 0];
% SVM (Space Vector Modulation) calculation
SVM_vec = [r_sinPhaA_M1sca; r_sinPhaB_M1sca; r_sinPhaC_M1sca];
SVM_min = min(SVM_vec);
SVM_max = max(SVM_vec);
SVM_sum = SVM_min + SVM_max;
SVM_vec = SVM_vec - 0.5*SVM_sum;
SVM_vec = (2/sqrt(3))*SVM_vec;
color = ['m' 'g' 'b'];
lw = 1.5;
figure
s1 = subplot(231); hold on
stairs(a_commElecAngle_XA, hall_A, color(1), 'Linewidth', lw);
stairs(a_commElecAngle_XA, hall_B, color(2), 'Linewidth', lw);
stairs(a_commElecAngle_XA, hall_C, color(3), 'Linewidth', lw);
xticks(a_commElecAngle_XA);
grid
yticks(0:5);
yticklabels({'0','1','0','1','0','1'});
title('Hall sensors');
legend('Phase A','Phase B','Phase C','Location','NorthEast');
s2 = subplot(232); hold on
stairs(a_commElecAngle_XA, hall_A, color(1), 'Linewidth', lw);
stairs(a_commElecAngle_XA, hall_B, color(2), 'Linewidth', lw);
stairs(a_commElecAngle_XA, hall_C, color(3), 'Linewidth', lw);
xticks(a_commElecAngle_XA);
grid
yticks(0:5);
yticklabels({'0','1','0','1','0','1'});
title('Hall sensors');
legend('Phase A','Phase B','Phase C','Location','NorthEast');
s3 = subplot(233); hold on
stairs(a_commElecAngle_XA, hall_A, color(1), 'Linewidth', lw);
stairs(a_commElecAngle_XA, hall_B, color(2), 'Linewidth', lw);
stairs(a_commElecAngle_XA, hall_C, color(3), 'Linewidth', lw);
xticks(a_commElecAngle_XA);
grid
yticks(0:5);
yticklabels({'0','1','0','1','0','1'});
title('Hall sensors');
legend('Phase A','Phase B','Phase C','Location','NorthEast');
s4 = subplot(234); hold on
stairs(a_commElecAngle_XA, sca_factor*[z_commutMap_M1(1,:) z_commutMap_M1(1,1)] + 6000, color(1), 'Linewidth', lw);
stairs(a_commElecAngle_XA, sca_factor*[z_commutMap_M1(2,:) z_commutMap_M1(2,1)] + 3000, color(2), 'Linewidth', lw);
stairs(a_commElecAngle_XA, sca_factor*[z_commutMap_M1(3,:) z_commutMap_M1(3,1)], color(3), 'Linewidth', lw);
xticks(a_commElecAngle_XA);
yticks(-1000:1000:7000);
yticklabels({'-1000','0','1000','-1000','0','1000','-1000','0','1000'});
ylim([-1000 7000]);
grid
title('Commutation method [0]');
xlabel('Electrical angle [deg]');
s5 = subplot(235); hold on
plot(a_elecAngle_XA, r_sin3PhaA_M1sca, color(1), 'Linewidth', lw);
plot(a_elecAngle_XA, r_sin3PhaB_M1sca, color(2), 'Linewidth', lw);
plot(a_elecAngle_XA, r_sin3PhaC_M1sca, color(3), 'Linewidth', lw);
xticks(a_commElecAngle_XA);
ylim([-1000 1000])
grid
title('SIN method [1]');
xlabel('Electrical angle [deg]');
s6 = subplot(236); hold on
plot(a_elecAngle_XA, SVM_vec(1,:), color(1), 'Linewidth', lw);
plot(a_elecAngle_XA, SVM_vec(2,:), color(2), 'Linewidth', lw);
plot(a_elecAngle_XA, SVM_vec(3,:), color(3), 'Linewidth', lw);
xticks(a_commElecAngle_XA);
ylim([-1000 1000])
grid
title('FOC method [2]');
xlabel('Electrical angle [deg]');
linkaxes([s1 s2 s3 s4 s5 s6],'x');
xlim([0 360]);
end