Skip to content

Commit 5038266

Browse files
runger1101001runger1101001
authored
Merge pull request #158 from runger1101001/dev
New debugging facility & stm32 driver refactoring
2 parents c2068a3 + 3b7b261 commit 5038266

16 files changed

+821
-311
lines changed

‎src/BLDCMotor.cpp

Lines changed: 28 additions & 28 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,5 @@
11
#include "BLDCMotor.h"
2+
#include "./communication/SimpleFOCDebug.h"
23

34
// BLDCMotor( int pp , float R)
45
// - pp - pole pair number
@@ -26,10 +27,11 @@ void BLDCMotor::linkDriver(BLDCDriver* _driver) {
2627
void BLDCMotor::init() {
2728
if (!driver || !driver->initialized) {
2829
motor_status = FOCMotorStatus::motor_init_failed;
29-
if(monitor_port) monitor_port->println(F("MOT: Init not possible, driver not initialized"));
30+
SIMPLEFOC_DEBUG("MOT: Init not possible, driver not initialized");
31+
return;
3032
}
3133
motor_status = FOCMotorStatus::motor_initializing;
32-
if(monitor_port) monitor_port->println(F("MOT: Init"));
34+
SIMPLEFOC_DEBUG("MOT: Init");
3335

3436
// if no current sensing and the user has set the phase resistance of the motor use current limit to calculate the voltage limit
3537
if( !current_sense && _isset(phase_resistance)) {
@@ -59,7 +61,7 @@ void BLDCMotor::init() {
5961

6062
_delay(500);
6163
// enable motor
62-
if(monitor_port) monitor_port->println(F("MOT: Enable driver."));
64+
SIMPLEFOC_DEBUG("MOT: Enable driver.");
6365
enable();
6466
_delay(500);
6567
motor_status = FOCMotorStatus::motor_uncalibrated;
@@ -113,24 +115,25 @@ int BLDCMotor::initFOC( float zero_electric_offset, Direction _sensor_direction
113115
// added the shaft_angle update
114116
sensor->update();
115117
shaft_angle = shaftAngle();
116-
}else if(monitor_port) monitor_port->println(F("MOT: No sensor."));
118+
}else
119+
SIMPLEFOC_DEBUG("MOT: No sensor.");
117120

118121
// aligning the current sensor - can be skipped
119122
// checks if driver phases are the same as current sense phases
120123
// and checks the direction of measuremnt.
121124
_delay(500);
122125
if(exit_flag){
123126
if(current_sense) exit_flag *= alignCurrentSense();
124-
else if(monitor_port) monitor_port->println(F("MOT: No current sense."));
127+
else SIMPLEFOC_DEBUG("MOT: No current sense.");
125128
}
126129

127130
if(exit_flag){
128-
if(monitor_port) monitor_port->println(F("MOT: Ready."));
131+
SIMPLEFOC_DEBUG("MOT: Ready.");
129132
motor_status = FOCMotorStatus::motor_ready;
130133
}else{
131-
if(monitor_port) monitor_port->println(F("MOT: Init FOC failed."));
132-
disable();
134+
SIMPLEFOC_DEBUG("MOT: Init FOC failed.");
133135
motor_status = FOCMotorStatus::motor_calib_failed;
136+
disable();
134137
}
135138

136139
return exit_flag;
@@ -140,18 +143,17 @@ int BLDCMotor::initFOC( float zero_electric_offset, Direction _sensor_direction
140143
int BLDCMotor::alignCurrentSense() {
141144
int exit_flag = 1; // success
142145

143-
if(monitor_port) monitor_port->println(F("MOT: Align current sense."));
146+
SIMPLEFOC_DEBUG("MOT: Align current sense.");
144147

145148
// align current sense and the driver
146149
exit_flag = current_sense->driverAlign(voltage_sensor_align);
147150
if(!exit_flag){
148151
// error in current sense - phase either not measured or bad connection
149-
if(monitor_port) monitor_port->println(F("MOT: Align error!"));
152+
SIMPLEFOC_DEBUG("MOT: Align error!");
150153
exit_flag = 0;
151154
}else{
152155
// output the alignment status flag
153-
if(monitor_port) monitor_port->print(F("MOT: Success: "));
154-
if(monitor_port) monitor_port->println(exit_flag);
156+
SIMPLEFOC_DEBUG("MOT: Success: ", exit_flag);
155157
}
156158

157159
return exit_flag > 0;
@@ -160,7 +162,7 @@ int BLDCMotor::alignCurrentSense() {
160162
// Encoder alignment to electrical 0 angle
161163
int BLDCMotor::alignSensor() {
162164
int exit_flag = 1; //success
163-
if(monitor_port) monitor_port->println(F("MOT: Align sensor."));
165+
SIMPLEFOC_DEBUG("MOT: Align sensor.");
164166

165167
// if unknown natural direction
166168
if(!_isset(sensor_direction)){
@@ -191,24 +193,23 @@ int BLDCMotor::alignSensor() {
191193
_delay(200);
192194
// determine the direction the sensor moved
193195
if (mid_angle == end_angle) {
194-
if(monitor_port) monitor_port->println(F("MOT: Failed to notice movement"));
196+
SIMPLEFOC_DEBUG("MOT: Failed to notice movement");
195197
return 0; // failed calibration
196198
} else if (mid_angle < end_angle) {
197-
if(monitor_port) monitor_port->println(F("MOT: sensor_direction==CCW"));
199+
SIMPLEFOC_DEBUG("MOT: sensor_direction==CCW");
198200
sensor_direction = Direction::CCW;
199201
} else{
200-
if(monitor_port) monitor_port->println(F("MOT: sensor_direction==CW"));
202+
SIMPLEFOC_DEBUG("MOT: sensor_direction==CW");
201203
sensor_direction = Direction::CW;
202204
}
203205
// check pole pair number
204-
if(monitor_port) monitor_port->print(F("MOT: PP check: "));
205206
float moved = fabs(mid_angle - end_angle);
206207
if( fabs(moved*pole_pairs - _2PI) > 0.5f ) { // 0.5f is arbitrary number it can be lower or higher!
207-
if(monitor_port) monitor_port->print(F("fail - estimated pp:"));
208-
if(monitor_port) monitor_port->println(_2PI/moved,4);
209-
}else if(monitor_port) monitor_port->println(F("OK!"));
208+
SIMPLEFOC_DEBUG("MOT: PP check: fail - estimated pp: ", _2PI/moved);
209+
} else
210+
SIMPLEFOC_DEBUG("MOT: PP check: OK!");
210211

211-
}else if(monitor_port) monitor_port->println(F("MOT: Skip dir calib."));
212+
} else SIMPLEFOC_DEBUG("MOT: Skip dir calib.");
212213

213214
// zero electric angle not known
214215
if(!_isset(zero_electric_angle)){
@@ -224,13 +225,12 @@ int BLDCMotor::alignSensor() {
224225
//zero_electric_angle = _normalizeAngle(_electricalAngle(sensor_direction*sensor->getAngle(), pole_pairs));
225226
_delay(20);
226227
if(monitor_port){
227-
monitor_port->print(F("MOT: Zero elec. angle: "));
228-
monitor_port->println(zero_electric_angle);
228+
SIMPLEFOC_DEBUG("MOT: Zero elec. angle: ", zero_electric_angle);
229229
}
230230
// stop everything
231231
setPhaseVoltage(0, 0, 0);
232232
_delay(200);
233-
}else if(monitor_port) monitor_port->println(F("MOT: Skip offset calib."));
233+
}else SIMPLEFOC_DEBUG("MOT: Skip offset calib.");
234234
return exit_flag;
235235
}
236236

@@ -239,7 +239,7 @@ int BLDCMotor::alignSensor() {
239239
int BLDCMotor::absoluteZeroSearch() {
240240
// sensor precision: this is all ok, as the search happens near the 0-angle, where the precision
241241
// of float is sufficient.
242-
if(monitor_port) monitor_port->println(F("MOT: Index search..."));
242+
SIMPLEFOC_DEBUG("MOT: Index search...");
243243
// search the absolute zero with small velocity
244244
float limit_vel = velocity_limit;
245245
float limit_volt = voltage_limit;
@@ -259,8 +259,8 @@ int BLDCMotor::absoluteZeroSearch() {
259259
voltage_limit = limit_volt;
260260
// check if the zero found
261261
if(monitor_port){
262-
if(sensor->needsSearch()) monitor_port->println(F("MOT: Error: Not found!"));
263-
else monitor_port->println(F("MOT: Success!"));
262+
if(sensor->needsSearch()) SIMPLEFOC_DEBUG("MOT: Error: Not found!");
263+
else SIMPLEFOC_DEBUG("MOT: Success!");
264264
}
265265
return !sensor->needsSearch();
266266
}
@@ -309,7 +309,7 @@ void BLDCMotor::loopFOC() {
309309
break;
310310
default:
311311
// no torque control selected
312-
if(monitor_port) monitor_port->println(F("MOT: no torque control selected!"));
312+
SIMPLEFOC_DEBUG("MOT: no torque control selected!");
313313
break;
314314
}
315315

‎src/SimpleFOC.h

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -114,5 +114,6 @@ void loop() {
114114
#include "current_sense/GenericCurrentSense.h"
115115
#include "communication/Commander.h"
116116
#include "communication/StepDirListener.h"
117+
#include "communication/SimpleFOCDebug.h"
117118

118119
#endif

‎src/StepperMotor.cpp

Lines changed: 22 additions & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,6 @@
11
#include "StepperMotor.h"
2+
#include "./communication/SimpleFOCDebug.h"
3+
24

35
// StepperMotor(int pp)
46
// - pp - pole pair number
@@ -27,10 +29,11 @@ void StepperMotor::linkDriver(StepperDriver* _driver) {
2729
void StepperMotor::init() {
2830
if (!driver || !driver->initialized) {
2931
motor_status = FOCMotorStatus::motor_init_failed;
30-
if(monitor_port) monitor_port->println(F("MOT: Init not possible, driver not initialized"));
32+
SIMPLEFOC_DEBUG("MOT: Init not possible, driver not initialized");
33+
return;
3134
}
3235
motor_status = FOCMotorStatus::motor_initializing;
33-
if(monitor_port) monitor_port->println(F("MOT: Init"));
36+
SIMPLEFOC_DEBUG("MOT: Init");
3437

3538
// if set the phase resistance of the motor use current limit to calculate the voltage limit
3639
if(_isset(phase_resistance)) {
@@ -54,7 +57,7 @@ void StepperMotor::init() {
5457

5558
_delay(500);
5659
// enable motor
57-
if(monitor_port) monitor_port->println(F("MOT: Enable driver."));
60+
SIMPLEFOC_DEBUG("MOT: Enable driver.");
5861
enable();
5962
_delay(500);
6063

@@ -110,13 +113,13 @@ int StepperMotor::initFOC( float zero_electric_offset, Direction _sensor_direct
110113
// added the shaft_angle update
111114
sensor->update();
112115
shaft_angle = sensor->getAngle();
113-
}else if(monitor_port) monitor_port->println(F("MOT: No sensor."));
116+
}else SIMPLEFOC_DEBUG("MOT: No sensor.");
114117

115118
if(exit_flag){
116-
if(monitor_port) monitor_port->println(F("MOT: Ready."));
119+
SIMPLEFOC_DEBUG("MOT: Ready.");
117120
motor_status = FOCMotorStatus::motor_ready;
118121
}else{
119-
if(monitor_port) monitor_port->println(F("MOT: Init FOC failed."));
122+
SIMPLEFOC_DEBUG("MOT: Init FOC failed.");
120123
motor_status = FOCMotorStatus::motor_calib_failed;
121124
disable();
122125
}
@@ -127,7 +130,7 @@ int StepperMotor::initFOC( float zero_electric_offset, Direction _sensor_direct
127130
// Encoder alignment to electrical 0 angle
128131
int StepperMotor::alignSensor() {
129132
int exit_flag = 1; //success
130-
if(monitor_port) monitor_port->println(F("MOT: Align sensor."));
133+
SIMPLEFOC_DEBUG("MOT: Align sensor.");
131134

132135
// if unknown natural direction
133136
if(!_isset(sensor_direction)){
@@ -158,24 +161,23 @@ int StepperMotor::alignSensor() {
158161
_delay(200);
159162
// determine the direction the sensor moved
160163
if (mid_angle == end_angle) {
161-
if(monitor_port) monitor_port->println(F("MOT: Failed to notice movement"));
164+
SIMPLEFOC_DEBUG("MOT: Failed to notice movement");
162165
return 0; // failed calibration
163166
} else if (mid_angle < end_angle) {
164-
if(monitor_port) monitor_port->println(F("MOT: sensor_direction==CCW"));
167+
SIMPLEFOC_DEBUG("MOT: sensor_direction==CCW");
165168
sensor_direction = Direction::CCW;
166169
} else{
167-
if(monitor_port) monitor_port->println(F("MOT: sensor_direction==CW"));
170+
SIMPLEFOC_DEBUG("MOT: sensor_direction==CW");
168171
sensor_direction = Direction::CW;
169172
}
170173
// check pole pair number
171-
if(monitor_port) monitor_port->print(F("MOT: PP check: "));
172174
float moved = fabs(mid_angle - end_angle);
173175
if( fabs(moved*pole_pairs - _2PI) > 0.5f ) { // 0.5f is arbitrary number it can be lower or higher!
174-
if(monitor_port) monitor_port->print(F("fail - estimated pp:"));
175-
if(monitor_port) monitor_port->println(_2PI/moved,4);
176-
}else if(monitor_port) monitor_port->println(F("OK!"));
176+
SIMPLEFOC_DEBUG("MOT: PP check: fail - estimated pp: ", _2PI/moved);
177+
} else
178+
SIMPLEFOC_DEBUG("MOT: PP check: OK!");
177179

178-
}else if(monitor_port) monitor_port->println(F("MOT: Skip dir calib."));
180+
}else SIMPLEFOC_DEBUG("MOT: Skip dir calib.");
179181

180182
// zero electric angle not known
181183
if(!_isset(zero_electric_angle)){
@@ -190,21 +192,20 @@ int StepperMotor::alignSensor() {
190192
zero_electric_angle = electricalAngle();
191193
_delay(20);
192194
if(monitor_port){
193-
monitor_port->print(F("MOT: Zero elec. angle: "));
194-
monitor_port->println(zero_electric_angle);
195+
SIMPLEFOC_DEBUG("MOT: Zero elec. angle: ", zero_electric_angle);
195196
}
196197
// stop everything
197198
setPhaseVoltage(0, 0, 0);
198199
_delay(200);
199-
}else if(monitor_port) monitor_port->println(F("MOT: Skip offset calib."));
200+
}else SIMPLEFOC_DEBUG("MOT: Skip offset calib.");
200201
return exit_flag;
201202
}
202203

203204
// Encoder alignment the absolute zero angle
204205
// - to the index
205206
int StepperMotor::absoluteZeroSearch() {
206207

207-
if(monitor_port) monitor_port->println(F("MOT: Index search..."));
208+
SIMPLEFOC_DEBUG("MOT: Index search...");
208209
// search the absolute zero with small velocity
209210
float limit_vel = velocity_limit;
210211
float limit_volt = voltage_limit;
@@ -224,8 +225,8 @@ int StepperMotor::absoluteZeroSearch() {
224225
voltage_limit = limit_volt;
225226
// check if the zero found
226227
if(monitor_port){
227-
if(sensor->needsSearch()) monitor_port->println(F("MOT: Error: Not found!"));
228-
else monitor_port->println(F("MOT: Success!"));
228+
if(sensor->needsSearch()) SIMPLEFOC_DEBUG("MOT: Error: Not found!");
229+
else SIMPLEFOC_DEBUG("MOT: Success!");
229230
}
230231
return !sensor->needsSearch();
231232
}

‎src/common/base_classes/FOCMotor.cpp

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,5 @@
11
#include "FOCMotor.h"
2+
#include "../../communication/SimpleFOCDebug.h"
23

34
/**
45
* Default constructor - setting all variabels to default values
@@ -77,7 +78,8 @@ float FOCMotor::electricalAngle(){
7778
// function implementing the monitor_port setter
7879
void FOCMotor::useMonitoring(Print &print){
7980
monitor_port = &print; //operate on the address of print
80-
if(monitor_port ) monitor_port->println(F("MOT: Monitor enabled!"));
81+
SimpleFOCDebug::enable(&print);
82+
SIMPLEFOC_DEBUG("MOT: Monitor enabled!");
8183
}
8284

8385
// utility function intended to be used with serial plotter to monitor motor variables

0 commit comments

Comments
 (0)