Skip to content

Commit 8748d40

Browse files
committed
Merge branch 'dev' of github.com:askuric/Arduino-FOC into dev
2 parents 00b22fb + 03ee308 commit 8748d40

File tree

1 file changed

+26
-24
lines changed

1 file changed

+26
-24
lines changed

src/BLDCMotor.cpp

Lines changed: 26 additions & 24 deletions
Original file line numberDiff line numberDiff line change
@@ -29,7 +29,7 @@ void BLDCMotor::linkDriver(BLDCDriver* _driver) {
2929
driver = _driver;
3030
}
3131

32-
// init hardware pins
32+
// init hardware pins
3333
void BLDCMotor::init() {
3434
if(monitor_port) monitor_port->println("MOT: Initialise variables.");
3535
// sanity check for the voltage limit configuration
@@ -53,13 +53,13 @@ void BLDCMotor::disable()
5353
{
5454
// set zero to PWM
5555
driver->setPwm(0, 0, 0);
56-
// disable the driver
56+
// disable the driver
5757
driver->disable();
5858
}
5959
// enable motor driver
6060
void BLDCMotor::enable()
6161
{
62-
// enable the driver
62+
// enable the driver
6363
driver->enable();
6464
// set zero to PWM
6565
driver->setPwm(0, 0, 0);
@@ -93,7 +93,7 @@ int BLDCMotor::initFOC( float zero_electric_offset, Direction sensor_direction
9393
int BLDCMotor::alignSensor() {
9494
if(monitor_port) monitor_port->println("MOT: Align sensor.");
9595
// align the electrical phases of the motor and sensor
96-
// set angle -90 degrees
96+
// set angle -90 degrees
9797

9898
float start_angle = shaftAngle();
9999
for (int i = 0; i <=5; i++ ) {
@@ -112,6 +112,8 @@ int BLDCMotor::alignSensor() {
112112
sensor->natural_direction = Direction::CCW;
113113
} else if (mid_angle == start_angle) {
114114
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");
115117
}
116118

117119
// let the motor stabilize for 2 sec
@@ -134,19 +136,19 @@ int BLDCMotor::alignSensor() {
134136
}
135137

136138

137-
// Encoder alignment the absolute zero angle
139+
// Encoder alignment the absolute zero angle
138140
// - to the index
139141
int BLDCMotor::absoluteZeroAlign() {
140142

141143
if(monitor_port) monitor_port->println("MOT: Absolute zero align.");
142144
// if no absolute zero return
143145
if(!sensor->hasAbsoluteZero()) return 0;
144-
146+
145147

146148
if(monitor_port && sensor->needsAbsoluteZeroSearch()) monitor_port->println("MOT: Searching...");
147149
// search the absolute zero with small velocity
148150
while(sensor->needsAbsoluteZeroSearch() && shaft_angle < _2PI){
149-
loopFOC();
151+
loopFOC();
150152
voltage_q = PID_velocity(velocity_index_search - shaftVelocity());
151153
}
152154
voltage_q = 0;
@@ -167,9 +169,9 @@ int BLDCMotor::absoluteZeroAlign() {
167169
// Iterative function looping FOC algorithm, setting Uq on the Motor
168170
// The faster it can be run the better
169171
void BLDCMotor::loopFOC() {
170-
// shaft angle
172+
// shaft angle
171173
shaft_angle = shaftAngle();
172-
// set the phase voltage - FOC heart function :)
174+
// set the phase voltage - FOC heart function :)
173175
setPhaseVoltage(voltage_q, voltage_d, _electricalAngle(shaft_angle,pole_pairs));
174176
}
175177

@@ -219,7 +221,7 @@ void BLDCMotor::move(float new_target) {
219221

220222
// Method using FOC to set Uq and Ud to the motor at the optimal angle
221223
// Function implementing Space Vector PWM and Sine PWM algorithms
222-
//
224+
//
223225
// Function using sine approximation
224226
// regular sin + cos ~300us (no memory usaage)
225227
// approx _sin + _cos ~110us (400Byte ~ 20% of memory)
@@ -257,7 +259,7 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) {
257259
};
258260
// static int trap_150_state = 0;
259261
sector = 12 * (_normalizeAngle(angle_el + PI/6.0 + zero_electric_angle) / _2PI); // adding PI/6 to align with other modes
260-
262+
261263
Ua = Uq + trap_150_map[sector][0] * Uq;
262264
Ub = Uq + trap_150_map[sector][1] * Uq;
263265
Uc = Uq + trap_150_map[sector][2] * Uq;
@@ -272,7 +274,7 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) {
272274
break;
273275

274276
case FOCModulationType::SinePWM :
275-
// Sinusoidal PWM modulation
277+
// Sinusoidal PWM modulation
276278
// Inverse Park + Clarke transformation
277279

278280
// angle normalization in between 0 and 2pi
@@ -299,10 +301,10 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) {
299301
break;
300302

301303
case FOCModulationType::SpaceVectorPWM :
302-
// Nice video explaining the SpaceVectorModulation (SVPWM) algorithm
304+
// Nice video explaining the SpaceVectorModulation (SVPWM) algorithm
303305
// https://www.youtube.com/watch?v=QMSWUMEAejg
304306

305-
// if negative voltages change inverse the phase
307+
// if negative voltages change inverse the phase
306308
// angle + 180degrees
307309
if(Uq < 0) angle_el += _PI;
308310
Uq = abs(Uq);
@@ -316,14 +318,14 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) {
316318
// calculate the duty cycles
317319
float T1 = _SQRT3*_sin(sector*_PI_3 - angle_el) * Uq/driver->voltage_limit;
318320
float T2 = _SQRT3*_sin(angle_el - (sector-1.0)*_PI_3) * Uq/driver->voltage_limit;
319-
// two versions possible
321+
// two versions possible
320322
float T0 = 0; // pulled to 0 - better for low power supply voltage
321-
if (centered) {
323+
if (centered) {
322324
T0 = 1 - T1 - T2; //centered around driver->voltage_limit/2
323-
}
325+
}
324326

325327
// calculate the duty cycles(times)
326-
float Ta,Tb,Tc;
328+
float Ta,Tb,Tc;
327329
switch(sector){
328330
case 1:
329331
Ta = T1 + T2 + T0/2;
@@ -369,7 +371,7 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) {
369371
break;
370372

371373
}
372-
374+
373375
// set the voltages in driver
374376
driver->setPwm(Ua, Ub, Uc);
375377
}
@@ -386,7 +388,7 @@ void BLDCMotor::velocityOpenloop(float target_velocity){
386388
float Ts = (now_us - open_loop_timestamp) * 1e-6;
387389

388390
// calculate the necessary angle to achieve target velocity
389-
shaft_angle += target_velocity*Ts;
391+
shaft_angle += target_velocity*Ts;
390392

391393
// set the maximal allowed voltage (voltage_limit) with the necessary angle
392394
setPhaseVoltage(voltage_limit, 0, _electricalAngle(shaft_angle, pole_pairs));
@@ -403,17 +405,17 @@ void BLDCMotor::angleOpenloop(float target_angle){
403405
unsigned long now_us = _micros();
404406
// calculate the sample time from last call
405407
float Ts = (now_us - open_loop_timestamp) * 1e-6;
406-
408+
407409
// calculate the necessary angle to move from current position towards target angle
408410
// with maximal velocity (velocity_limit)
409411
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;
411413
else
412414
shaft_angle = target_angle;
413-
415+
414416
// set the maximal allowed voltage (voltage_limit) with the necessary angle
415417
setPhaseVoltage(voltage_limit, 0, _electricalAngle(shaft_angle, pole_pairs));
416418

417419
// save timestamp for next call
418420
open_loop_timestamp = now_us;
419-
}
421+
}

0 commit comments

Comments
 (0)