@@ -29,7 +29,7 @@ void BLDCMotor::linkDriver(BLDCDriver* _driver) {
29
29
driver = _driver;
30
30
}
31
31
32
- // init hardware pins
32
+ // init hardware pins
33
33
void BLDCMotor::init () {
34
34
if (monitor_port) monitor_port->println (" MOT: Initialise variables." );
35
35
// sanity check for the voltage limit configuration
@@ -53,13 +53,13 @@ void BLDCMotor::disable()
53
53
{
54
54
// set zero to PWM
55
55
driver->setPwm (0 , 0 , 0 );
56
- // disable the driver
56
+ // disable the driver
57
57
driver->disable ();
58
58
}
59
59
// enable motor driver
60
60
void BLDCMotor::enable ()
61
61
{
62
- // enable the driver
62
+ // enable the driver
63
63
driver->enable ();
64
64
// set zero to PWM
65
65
driver->setPwm (0 , 0 , 0 );
@@ -93,7 +93,7 @@ int BLDCMotor::initFOC( float zero_electric_offset, Direction sensor_direction
93
93
int BLDCMotor::alignSensor () {
94
94
if (monitor_port) monitor_port->println (" MOT: Align sensor." );
95
95
// align the electrical phases of the motor and sensor
96
- // set angle -90 degrees
96
+ // set angle -90 degrees
97
97
98
98
float start_angle = shaftAngle ();
99
99
for (int i = 0 ; i <=5 ; i++ ) {
@@ -112,6 +112,8 @@ int BLDCMotor::alignSensor() {
112
112
sensor->natural_direction = Direction::CCW;
113
113
} else if (mid_angle == start_angle) {
114
114
if (monitor_port) monitor_port->println (" MOT: Sensor failed to notice movement" );
115
+ } else {
116
+ if (monitor_port) monitor_port->println (" MOT: natural_direction==CW" );
115
117
}
116
118
117
119
// let the motor stabilize for 2 sec
@@ -134,19 +136,19 @@ int BLDCMotor::alignSensor() {
134
136
}
135
137
136
138
137
- // Encoder alignment the absolute zero angle
139
+ // Encoder alignment the absolute zero angle
138
140
// - to the index
139
141
int BLDCMotor::absoluteZeroAlign () {
140
142
141
143
if (monitor_port) monitor_port->println (" MOT: Absolute zero align." );
142
144
// if no absolute zero return
143
145
if (!sensor->hasAbsoluteZero ()) return 0 ;
144
-
146
+
145
147
146
148
if (monitor_port && sensor->needsAbsoluteZeroSearch ()) monitor_port->println (" MOT: Searching..." );
147
149
// search the absolute zero with small velocity
148
150
while (sensor->needsAbsoluteZeroSearch () && shaft_angle < _2PI){
149
- loopFOC ();
151
+ loopFOC ();
150
152
voltage_q = PID_velocity (velocity_index_search - shaftVelocity ());
151
153
}
152
154
voltage_q = 0 ;
@@ -167,9 +169,9 @@ int BLDCMotor::absoluteZeroAlign() {
167
169
// Iterative function looping FOC algorithm, setting Uq on the Motor
168
170
// The faster it can be run the better
169
171
void BLDCMotor::loopFOC () {
170
- // shaft angle
172
+ // shaft angle
171
173
shaft_angle = shaftAngle ();
172
- // set the phase voltage - FOC heart function :)
174
+ // set the phase voltage - FOC heart function :)
173
175
setPhaseVoltage (voltage_q, voltage_d, _electricalAngle (shaft_angle,pole_pairs));
174
176
}
175
177
@@ -219,7 +221,7 @@ void BLDCMotor::move(float new_target) {
219
221
220
222
// Method using FOC to set Uq and Ud to the motor at the optimal angle
221
223
// Function implementing Space Vector PWM and Sine PWM algorithms
222
- //
224
+ //
223
225
// Function using sine approximation
224
226
// regular sin + cos ~300us (no memory usaage)
225
227
// approx _sin + _cos ~110us (400Byte ~ 20% of memory)
@@ -257,7 +259,7 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) {
257
259
};
258
260
// static int trap_150_state = 0;
259
261
sector = 12 * (_normalizeAngle (angle_el + PI/6.0 + zero_electric_angle) / _2PI); // adding PI/6 to align with other modes
260
-
262
+
261
263
Ua = Uq + trap_150_map[sector][0 ] * Uq;
262
264
Ub = Uq + trap_150_map[sector][1 ] * Uq;
263
265
Uc = Uq + trap_150_map[sector][2 ] * Uq;
@@ -272,7 +274,7 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) {
272
274
break ;
273
275
274
276
case FOCModulationType::SinePWM :
275
- // Sinusoidal PWM modulation
277
+ // Sinusoidal PWM modulation
276
278
// Inverse Park + Clarke transformation
277
279
278
280
// angle normalization in between 0 and 2pi
@@ -299,10 +301,10 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) {
299
301
break ;
300
302
301
303
case FOCModulationType::SpaceVectorPWM :
302
- // Nice video explaining the SpaceVectorModulation (SVPWM) algorithm
304
+ // Nice video explaining the SpaceVectorModulation (SVPWM) algorithm
303
305
// https://www.youtube.com/watch?v=QMSWUMEAejg
304
306
305
- // if negative voltages change inverse the phase
307
+ // if negative voltages change inverse the phase
306
308
// angle + 180degrees
307
309
if (Uq < 0 ) angle_el += _PI;
308
310
Uq = abs (Uq);
@@ -316,14 +318,14 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) {
316
318
// calculate the duty cycles
317
319
float T1 = _SQRT3*_sin (sector*_PI_3 - angle_el) * Uq/driver->voltage_limit ;
318
320
float T2 = _SQRT3*_sin (angle_el - (sector-1.0 )*_PI_3) * Uq/driver->voltage_limit ;
319
- // two versions possible
321
+ // two versions possible
320
322
float T0 = 0 ; // pulled to 0 - better for low power supply voltage
321
- if (centered) {
323
+ if (centered) {
322
324
T0 = 1 - T1 - T2; // centered around driver->voltage_limit/2
323
- }
325
+ }
324
326
325
327
// calculate the duty cycles(times)
326
- float Ta,Tb,Tc;
328
+ float Ta,Tb,Tc;
327
329
switch (sector){
328
330
case 1 :
329
331
Ta = T1 + T2 + T0/2 ;
@@ -369,7 +371,7 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) {
369
371
break ;
370
372
371
373
}
372
-
374
+
373
375
// set the voltages in driver
374
376
driver->setPwm (Ua, Ub, Uc);
375
377
}
@@ -386,7 +388,7 @@ void BLDCMotor::velocityOpenloop(float target_velocity){
386
388
float Ts = (now_us - open_loop_timestamp) * 1e-6 ;
387
389
388
390
// calculate the necessary angle to achieve target velocity
389
- shaft_angle += target_velocity*Ts;
391
+ shaft_angle += target_velocity*Ts;
390
392
391
393
// set the maximal allowed voltage (voltage_limit) with the necessary angle
392
394
setPhaseVoltage (voltage_limit, 0 , _electricalAngle (shaft_angle, pole_pairs));
@@ -403,17 +405,17 @@ void BLDCMotor::angleOpenloop(float target_angle){
403
405
unsigned long now_us = _micros ();
404
406
// calculate the sample time from last call
405
407
float Ts = (now_us - open_loop_timestamp) * 1e-6 ;
406
-
408
+
407
409
// calculate the necessary angle to move from current position towards target angle
408
410
// with maximal velocity (velocity_limit)
409
411
if (abs ( target_angle - shaft_angle ) > abs (velocity_limit*Ts))
410
- shaft_angle += _sign (target_angle - shaft_angle) * abs ( velocity_limit )*Ts;
412
+ shaft_angle += _sign (target_angle - shaft_angle) * abs ( velocity_limit )*Ts;
411
413
else
412
414
shaft_angle = target_angle;
413
-
415
+
414
416
// set the maximal allowed voltage (voltage_limit) with the necessary angle
415
417
setPhaseVoltage (voltage_limit, 0 , _electricalAngle (shaft_angle, pole_pairs));
416
418
417
419
// save timestamp for next call
418
420
open_loop_timestamp = now_us;
419
- }
421
+ }
0 commit comments