Skip to content

Commit aefee3f

Browse files
runger1101001runger1101001
committed
initial debug commit for merge
1 parent e80c72f commit aefee3f

File tree

4 files changed

+169
-46
lines changed

4 files changed

+169
-46
lines changed

‎src/BLDCMotor.cpp

Lines changed: 24 additions & 26 deletions
Original file line numberDiff line numberDiff line change
@@ -24,7 +24,7 @@ void BLDCMotor::linkDriver(BLDCDriver* _driver) {
2424

2525
// init hardware pins
2626
void BLDCMotor::init() {
27-
if(monitor_port) monitor_port->println(F("MOT: Init"));
27+
SIMPLEFOC_DEBUG("MOT: Init");
2828

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

5555
_delay(500);
5656
// enable motor
57-
if(monitor_port) monitor_port->println(F("MOT: Enable driver."));
57+
SIMPLEFOC_DEBUG("MOT: Enable driver.");
5858
enable();
5959
_delay(500);
6060
}
@@ -104,21 +104,22 @@ int BLDCMotor::initFOC( float zero_electric_offset, Direction _sensor_direction
104104
// added the shaft_angle update
105105
sensor->update();
106106
shaft_angle = shaftAngle();
107-
}else if(monitor_port) monitor_port->println(F("MOT: No sensor."));
107+
}else
108+
SIMPLEFOC_DEBUG("MOT: No sensor.");
108109

109110
// aligning the current sensor - can be skipped
110111
// checks if driver phases are the same as current sense phases
111112
// and checks the direction of measuremnt.
112113
_delay(500);
113114
if(exit_flag){
114115
if(current_sense) exit_flag *= alignCurrentSense();
115-
else if(monitor_port) monitor_port->println(F("MOT: No current sense."));
116+
else SIMPLEFOC_DEBUG("MOT: No current sense.");
116117
}
117118

118119
if(exit_flag){
119-
if(monitor_port) monitor_port->println(F("MOT: Ready."));
120+
SIMPLEFOC_DEBUG("MOT: Ready.");
120121
}else{
121-
if(monitor_port) monitor_port->println(F("MOT: Init FOC failed."));
122+
SIMPLEFOC_DEBUG("MOT: Init FOC failed.");
122123
disable();
123124
}
124125

@@ -129,18 +130,17 @@ int BLDCMotor::initFOC( float zero_electric_offset, Direction _sensor_direction
129130
int BLDCMotor::alignCurrentSense() {
130131
int exit_flag = 1; // success
131132

132-
if(monitor_port) monitor_port->println(F("MOT: Align current sense."));
133+
SIMPLEFOC_DEBUG("MOT: Align current sense.");
133134

134135
// align current sense and the driver
135136
exit_flag = current_sense->driverAlign(driver, voltage_sensor_align);
136137
if(!exit_flag){
137138
// error in current sense - phase either not measured or bad connection
138-
if(monitor_port) monitor_port->println(F("MOT: Align error!"));
139+
SIMPLEFOC_DEBUG("MOT: Align error!");
139140
exit_flag = 0;
140141
}else{
141142
// output the alignment status flag
142-
if(monitor_port) monitor_port->print(F("MOT: Success: "));
143-
if(monitor_port) monitor_port->println(exit_flag);
143+
SIMPLEFOC_DEBUG("MOT: Success: ", exit_flag);
144144
}
145145

146146
return exit_flag > 0;
@@ -149,7 +149,7 @@ int BLDCMotor::alignCurrentSense() {
149149
// Encoder alignment to electrical 0 angle
150150
int BLDCMotor::alignSensor() {
151151
int exit_flag = 1; //success
152-
if(monitor_port) monitor_port->println(F("MOT: Align sensor."));
152+
SIMPLEFOC_DEBUG("MOT: Align sensor.");
153153

154154
// if unknown natural direction
155155
if(!_isset(sensor_direction)){
@@ -180,24 +180,23 @@ int BLDCMotor::alignSensor() {
180180
_delay(200);
181181
// determine the direction the sensor moved
182182
if (mid_angle == end_angle) {
183-
if(monitor_port) monitor_port->println(F("MOT: Failed to notice movement"));
183+
SIMPLEFOC_DEBUG("MOT: Failed to notice movement");
184184
return 0; // failed calibration
185185
} else if (mid_angle < end_angle) {
186-
if(monitor_port) monitor_port->println(F("MOT: sensor_direction==CCW"));
186+
SIMPLEFOC_DEBUG("MOT: sensor_direction==CCW");
187187
sensor_direction = Direction::CCW;
188188
} else{
189-
if(monitor_port) monitor_port->println(F("MOT: sensor_direction==CW"));
189+
SIMPLEFOC_DEBUG("MOT: sensor_direction==CW");
190190
sensor_direction = Direction::CW;
191191
}
192192
// check pole pair number
193-
if(monitor_port) monitor_port->print(F("MOT: PP check: "));
194193
float moved = fabs(mid_angle - end_angle);
195194
if( fabs(moved*pole_pairs - _2PI) > 0.5f ) { // 0.5f is arbitrary number it can be lower or higher!
196-
if(monitor_port) monitor_port->print(F("fail - estimated pp:"));
197-
if(monitor_port) monitor_port->println(_2PI/moved,4);
198-
}else if(monitor_port) monitor_port->println(F("OK!"));
195+
SIMPLEFOC_DEBUG("MOT: PP check: fail - estimated pp: ", _2PI/moved);
196+
} else
197+
SIMPLEFOC_DEBUG("MOT: PP check: OK!");
199198

200-
}else if(monitor_port) monitor_port->println(F("MOT: Skip dir calib."));
199+
} else SIMPLEFOC_DEBUG("MOT: Skip dir calib.");
201200

202201
// zero electric angle not known
203202
if(!_isset(zero_electric_angle)){
@@ -213,13 +212,12 @@ int BLDCMotor::alignSensor() {
213212
//zero_electric_angle = _normalizeAngle(_electricalAngle(sensor_direction*sensor->getAngle(), pole_pairs));
214213
_delay(20);
215214
if(monitor_port){
216-
monitor_port->print(F("MOT: Zero elec. angle: "));
217-
monitor_port->println(zero_electric_angle);
215+
SIMPLEFOC_DEBUG("MOT: Zero elec. angle: ", zero_electric_angle);
218216
}
219217
// stop everything
220218
setPhaseVoltage(0, 0, 0);
221219
_delay(200);
222-
}else if(monitor_port) monitor_port->println(F("MOT: Skip offset calib."));
220+
}else SIMPLEFOC_DEBUG("MOT: Skip offset calib.");
223221
return exit_flag;
224222
}
225223

@@ -228,7 +226,7 @@ int BLDCMotor::alignSensor() {
228226
int BLDCMotor::absoluteZeroSearch() {
229227
// sensor precision: this is all ok, as the search happens near the 0-angle, where the precision
230228
// of float is sufficient.
231-
if(monitor_port) monitor_port->println(F("MOT: Index search..."));
229+
SIMPLEFOC_DEBUG("MOT: Index search...");
232230
// search the absolute zero with small velocity
233231
float limit_vel = velocity_limit;
234232
float limit_volt = voltage_limit;
@@ -248,8 +246,8 @@ int BLDCMotor::absoluteZeroSearch() {
248246
voltage_limit = limit_volt;
249247
// check if the zero found
250248
if(monitor_port){
251-
if(sensor->needsSearch()) monitor_port->println(F("MOT: Error: Not found!"));
252-
else monitor_port->println(F("MOT: Success!"));
249+
if(sensor->needsSearch()) SIMPLEFOC_DEBUG("MOT: Error: Not found!");
250+
else SIMPLEFOC_DEBUG("MOT: Success!");
253251
}
254252
return !sensor->needsSearch();
255253
}
@@ -298,7 +296,7 @@ void BLDCMotor::loopFOC() {
298296
break;
299297
default:
300298
// no torque control selected
301-
if(monitor_port) monitor_port->println(F("MOT: no torque control selected!"));
299+
SIMPLEFOC_DEBUG("MOT: no torque control selected!");
302300
break;
303301
}
304302

‎src/StepperMotor.cpp

Lines changed: 18 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -25,7 +25,7 @@ void StepperMotor::linkDriver(StepperDriver* _driver) {
2525

2626
// init hardware pins
2727
void StepperMotor::init() {
28-
if(monitor_port) monitor_port->println(F("MOT: Init"));
28+
SIMPLEFOC_DEBUG("MOT: Init");
2929

3030
// if set the phase resistance of the motor use current limit to calculate the voltage limit
3131
if(_isset(phase_resistance)) {
@@ -49,7 +49,7 @@ void StepperMotor::init() {
4949

5050
_delay(500);
5151
// enable motor
52-
if(monitor_port) monitor_port->println(F("MOT: Enable driver."));
52+
SIMPLEFOC_DEBUG("MOT: Enable driver.");
5353
enable();
5454
_delay(500);
5555

@@ -101,12 +101,12 @@ int StepperMotor::initFOC( float zero_electric_offset, Direction _sensor_direct
101101
// added the shaft_angle update
102102
sensor->update();
103103
shaft_angle = sensor->getAngle();
104-
}else if(monitor_port) monitor_port->println(F("MOT: No sensor."));
104+
}else SIMPLEFOC_DEBUG("MOT: No sensor.");
105105

106106
if(exit_flag){
107-
if(monitor_port) monitor_port->println(F("MOT: Ready."));
107+
SIMPLEFOC_DEBUG("MOT: Ready.");
108108
}else{
109-
if(monitor_port) monitor_port->println(F("MOT: Init FOC failed."));
109+
SIMPLEFOC_DEBUG("MOT: Init FOC failed.");
110110
disable();
111111
}
112112

@@ -116,7 +116,7 @@ int StepperMotor::initFOC( float zero_electric_offset, Direction _sensor_direct
116116
// Encoder alignment to electrical 0 angle
117117
int StepperMotor::alignSensor() {
118118
int exit_flag = 1; //success
119-
if(monitor_port) monitor_port->println(F("MOT: Align sensor."));
119+
SIMPLEFOC_DEBUG("MOT: Align sensor.");
120120

121121
// if unknown natural direction
122122
if(!_isset(sensor_direction)){
@@ -147,24 +147,23 @@ int StepperMotor::alignSensor() {
147147
_delay(200);
148148
// determine the direction the sensor moved
149149
if (mid_angle == end_angle) {
150-
if(monitor_port) monitor_port->println(F("MOT: Failed to notice movement"));
150+
SIMPLEFOC_DEBUG("MOT: Failed to notice movement");
151151
return 0; // failed calibration
152152
} else if (mid_angle < end_angle) {
153-
if(monitor_port) monitor_port->println(F("MOT: sensor_direction==CCW"));
153+
SIMPLEFOC_DEBUG("MOT: sensor_direction==CCW");
154154
sensor_direction = Direction::CCW;
155155
} else{
156-
if(monitor_port) monitor_port->println(F("MOT: sensor_direction==CW"));
156+
SIMPLEFOC_DEBUG("MOT: sensor_direction==CW");
157157
sensor_direction = Direction::CW;
158158
}
159159
// check pole pair number
160-
if(monitor_port) monitor_port->print(F("MOT: PP check: "));
161160
float moved = fabs(mid_angle - end_angle);
162161
if( fabs(moved*pole_pairs - _2PI) > 0.5f ) { // 0.5f is arbitrary number it can be lower or higher!
163-
if(monitor_port) monitor_port->print(F("fail - estimated pp:"));
164-
if(monitor_port) monitor_port->println(_2PI/moved,4);
165-
}else if(monitor_port) monitor_port->println(F("OK!"));
162+
SIMPLEFOC_DEBUG("MOT: PP check: fail - estimated pp: ", _2PI/moved);
163+
} else
164+
SIMPLEFOC_DEBUG("MOT: PP check: OK!");
166165

167-
}else if(monitor_port) monitor_port->println(F("MOT: Skip dir calib."));
166+
}else SIMPLEFOC_DEBUG("MOT: Skip dir calib.");
168167

169168
// zero electric angle not known
170169
if(!_isset(zero_electric_angle)){
@@ -179,21 +178,20 @@ int StepperMotor::alignSensor() {
179178
zero_electric_angle = electricalAngle();
180179
_delay(20);
181180
if(monitor_port){
182-
monitor_port->print(F("MOT: Zero elec. angle: "));
183-
monitor_port->println(zero_electric_angle);
181+
SIMPLEFOC_DEBUG("MOT: Zero elec. angle: ", zero_electric_angle);
184182
}
185183
// stop everything
186184
setPhaseVoltage(0, 0, 0);
187185
_delay(200);
188-
}else if(monitor_port) monitor_port->println(F("MOT: Skip offset calib."));
186+
}else SIMPLEFOC_DEBUG("MOT: Skip offset calib.");
189187
return exit_flag;
190188
}
191189

192190
// Encoder alignment the absolute zero angle
193191
// - to the index
194192
int StepperMotor::absoluteZeroSearch() {
195193

196-
if(monitor_port) monitor_port->println(F("MOT: Index search..."));
194+
SIMPLEFOC_DEBUG("MOT: Index search...");
197195
// search the absolute zero with small velocity
198196
float limit_vel = velocity_limit;
199197
float limit_volt = voltage_limit;
@@ -213,8 +211,8 @@ int StepperMotor::absoluteZeroSearch() {
213211
voltage_limit = limit_volt;
214212
// check if the zero found
215213
if(monitor_port){
216-
if(sensor->needsSearch()) monitor_port->println(F("MOT: Error: Not found!"));
217-
else monitor_port->println(F("MOT: Success!"));
214+
if(sensor->needsSearch()) SIMPLEFOC_DEBUG("MOT: Error: Not found!");
215+
else SIMPLEFOC_DEBUG("MOT: Success!");
218216
}
219217
return !sensor->needsSearch();
220218
}

‎src/communication/SimpleFOCDebug.cpp

Lines changed: 56 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,56 @@
1+
2+
#include "SimpleFOCDebug.h"
3+
4+
#ifndef SIMPLEFOC_DISABLE_DEBUG
5+
6+
7+
Print* SimpleFOCDebug::_debugPrint = NULL;
8+
9+
10+
void SimpleFOCDebug::enable(Print* debugPrint) {
11+
_debugPrint = debugPrint;
12+
}
13+
14+
15+
16+
void SimpleFOCDebug::println(const char* str) {
17+
if (_debugPrint != NULL) {
18+
_debugPrint->println(str);
19+
}
20+
}
21+
22+
void SimpleFOCDebug::println(const __FlashStringHelper* str) {
23+
if (_debugPrint != NULL) {
24+
_debugPrint->println(str);
25+
}
26+
}
27+
28+
void SimpleFOCDebug::println(const char* str, float val) {
29+
if (_debugPrint != NULL) {
30+
_debugPrint->print(str);
31+
_debugPrint->println(val);
32+
}
33+
}
34+
35+
void SimpleFOCDebug::println(const __FlashStringHelper* str, float val) {
36+
if (_debugPrint != NULL) {
37+
_debugPrint->print(str);
38+
_debugPrint->println(val);
39+
}
40+
}
41+
42+
void SimpleFOCDebug::println(const char* str, int val) {
43+
if (_debugPrint != NULL) {
44+
_debugPrint->print(str);
45+
_debugPrint->println(val);
46+
}
47+
}
48+
49+
void SimpleFOCDebug::println(const __FlashStringHelper* str, int val) {
50+
if (_debugPrint != NULL) {
51+
_debugPrint->print(str);
52+
_debugPrint->println(val);
53+
}
54+
}
55+
56+
#endif

‎src/communication/SimpleFOCDebug.h

Lines changed: 71 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,71 @@
1+
2+
#ifndef __SIMPLEFOCDEBUG_H__
3+
#define __SIMPLEFOCDEBUG_H__
4+
5+
#include "Arduino.h"
6+
7+
8+
/**
9+
* SimpleFOCDebug class
10+
*
11+
* This class is used to print debug messages to a chosen output.
12+
* Currently, Print instances are supported as targets, e.g. serial port.
13+
*
14+
* Activate debug output globally by calling enable(), optionally passing
15+
* in a Print instance. If none is provided "Serial" is used by default.
16+
*
17+
* To produce debug output, use the macro SIMPLEFOC_DEBUG:
18+
* SIMPLEFOC_DEBUG("Debug message!");
19+
* SIMPLEFOC_DEBUG("a float value:", 123.456);
20+
* SIMPLEFOC_DEBUG("an integer value: ", 123);
21+
*
22+
* Keep debugging output short and simple. Some of our MCUs have limited
23+
* RAM and limited serial output capabilities.
24+
*
25+
* By default, the SIMPLEFOC_DEBUG macro uses the flash string helper to
26+
* help preserve memory on Arduino boards.
27+
*
28+
* You can also disable debug output completely. In this case all debug output
29+
* and the SimpleFOCDebug class is removed from the compiled code.
30+
* Add -DSIMPLEFOC_DISABLE_DEBUG to your compiler flags to disable debug in
31+
* this way.
32+
*
33+
**/
34+
35+
36+
#ifndef SIMPLEFOC_DISABLE_DEBUG
37+
38+
class SimpleFOCDebug {
39+
public:
40+
void enable(Print* debugPrint = Serial);
41+
42+
void println(const __FlashStringHelper* msg);
43+
void println(const char* msg);
44+
void println(const __FlashStringHelper* msg, float val);
45+
void println(const char* msg, float val);
46+
void println(const __FlashStringHelper* msg, int val);
47+
void println(const char* msg, int val);
48+
49+
protected:
50+
static Print* _debugPrint;
51+
};
52+
53+
54+
#define SIMPLEFOC_DEBUG(msg, ...) \
55+
SimpleFOCDebug::println(F(msg) __VA_OPT__(,) __VA_ARGS__)
56+
57+
58+
59+
60+
61+
#else //ifndef SIMPLEFOC_DISABLE_DEBUG
62+
63+
64+
65+
#define SIMPLEFOC_DEBUG(msg, ...)
66+
67+
68+
69+
#endif //ifndef SIMPLEFOC_DISABLE_DEBUG
70+
#endif
71+

0 commit comments

Comments
 (0)