Skip to content

New debugging facility & stm32 driver refactoring #158

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 9 commits into from
Mar 16, 2022
Merged

New debugging facility & stm32 driver refactoring #158

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
aefee3f
initial debug commit for merge
runger1101001 Feb 12, 2022
829de7f
Merge remote-tracking branch 'origin/dev' into dev
runger1101001 Feb 12, 2022
0ec82e3
New debug management independent of monitoring
runger1101001 Feb 12, 2022
00e550e
make the old behaviour the default
runger1101001 Feb 12, 2022
cac0484
set driver initialized flag
Feb 27, 2022
401ebe9
Merge branch 'dev' of https://github.com/simplefoc/Arduino-FOC into dev
Feb 27, 2022
0066b5e
STM32 driver refactoring
Mar 9, 2022
0051af4
stm32 driver: fix check for low-side channels
Mar 9, 2022
3b7b261
fix channel counting
Mar 10, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
56 changes: 28 additions & 28 deletions src/BLDCMotor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,5 @@
#include "BLDCMotor.h"
#include "./communication/SimpleFOCDebug.h"

// BLDCMotor( int pp , float R)
// - pp - pole pair number
Expand Down Expand Up @@ -26,10 +27,11 @@ void BLDCMotor::linkDriver(BLDCDriver* _driver) {
void BLDCMotor::init() {
if (!driver || !driver->initialized) {
motor_status = FOCMotorStatus::motor_init_failed;
if(monitor_port) monitor_port->println(F("MOT: Init not possible, driver not initialized"));
SIMPLEFOC_DEBUG("MOT: Init not possible, driver not initialized");
return;
}
motor_status = FOCMotorStatus::motor_initializing;
if(monitor_port) monitor_port->println(F("MOT: Init"));
SIMPLEFOC_DEBUG("MOT: Init");

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

_delay(500);
// enable motor
if(monitor_port) monitor_port->println(F("MOT: Enable driver."));
SIMPLEFOC_DEBUG("MOT: Enable driver.");
enable();
_delay(500);
motor_status = FOCMotorStatus::motor_uncalibrated;
Expand Down Expand Up @@ -113,24 +115,25 @@ int BLDCMotor::initFOC( float zero_electric_offset, Direction _sensor_direction
// added the shaft_angle update
sensor->update();
shaft_angle = shaftAngle();
}else if(monitor_port) monitor_port->println(F("MOT: No sensor."));
}else
SIMPLEFOC_DEBUG("MOT: No sensor.");

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

if(exit_flag){
if(monitor_port) monitor_port->println(F("MOT: Ready."));
SIMPLEFOC_DEBUG("MOT: Ready.");
motor_status = FOCMotorStatus::motor_ready;
}else{
if(monitor_port) monitor_port->println(F("MOT: Init FOC failed."));
disable();
SIMPLEFOC_DEBUG("MOT: Init FOC failed.");
motor_status = FOCMotorStatus::motor_calib_failed;
disable();
}

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

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

// align current sense and the driver
exit_flag = current_sense->driverAlign(voltage_sensor_align);
if(!exit_flag){
// error in current sense - phase either not measured or bad connection
if(monitor_port) monitor_port->println(F("MOT: Align error!"));
SIMPLEFOC_DEBUG("MOT: Align error!");
exit_flag = 0;
}else{
// output the alignment status flag
if(monitor_port) monitor_port->print(F("MOT: Success: "));
if(monitor_port) monitor_port->println(exit_flag);
SIMPLEFOC_DEBUG("MOT: Success: ", exit_flag);
}

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

// if unknown natural direction
if(!_isset(sensor_direction)){
Expand Down Expand Up @@ -191,24 +193,23 @@ int BLDCMotor::alignSensor() {
_delay(200);
// determine the direction the sensor moved
if (mid_angle == end_angle) {
if(monitor_port) monitor_port->println(F("MOT: Failed to notice movement"));
SIMPLEFOC_DEBUG("MOT: Failed to notice movement");
return 0; // failed calibration
} else if (mid_angle < end_angle) {
if(monitor_port) monitor_port->println(F("MOT: sensor_direction==CCW"));
SIMPLEFOC_DEBUG("MOT: sensor_direction==CCW");
sensor_direction = Direction::CCW;
} else{
if(monitor_port) monitor_port->println(F("MOT: sensor_direction==CW"));
SIMPLEFOC_DEBUG("MOT: sensor_direction==CW");
sensor_direction = Direction::CW;
}
// check pole pair number
if(monitor_port) monitor_port->print(F("MOT: PP check: "));
float moved = fabs(mid_angle - end_angle);
if( fabs(moved*pole_pairs - _2PI) > 0.5f ) { // 0.5f is arbitrary number it can be lower or higher!
if(monitor_port) monitor_port->print(F("fail - estimated pp:"));
if(monitor_port) monitor_port->println(_2PI/moved,4);
}else if(monitor_port) monitor_port->println(F("OK!"));
SIMPLEFOC_DEBUG("MOT: PP check: fail - estimated pp: ", _2PI/moved);
} else
SIMPLEFOC_DEBUG("MOT: PP check: OK!");

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

// zero electric angle not known
if(!_isset(zero_electric_angle)){
Expand All @@ -224,13 +225,12 @@ int BLDCMotor::alignSensor() {
//zero_electric_angle = _normalizeAngle(_electricalAngle(sensor_direction*sensor->getAngle(), pole_pairs));
_delay(20);
if(monitor_port){
monitor_port->print(F("MOT: Zero elec. angle: "));
monitor_port->println(zero_electric_angle);
SIMPLEFOC_DEBUG("MOT: Zero elec. angle: ", zero_electric_angle);
}
// stop everything
setPhaseVoltage(0, 0, 0);
_delay(200);
}else if(monitor_port) monitor_port->println(F("MOT: Skip offset calib."));
}else SIMPLEFOC_DEBUG("MOT: Skip offset calib.");
return exit_flag;
}

Expand All @@ -239,7 +239,7 @@ int BLDCMotor::alignSensor() {
int BLDCMotor::absoluteZeroSearch() {
// sensor precision: this is all ok, as the search happens near the 0-angle, where the precision
// of float is sufficient.
if(monitor_port) monitor_port->println(F("MOT: Index search..."));
SIMPLEFOC_DEBUG("MOT: Index search...");
// search the absolute zero with small velocity
float limit_vel = velocity_limit;
float limit_volt = voltage_limit;
Expand All @@ -259,8 +259,8 @@ int BLDCMotor::absoluteZeroSearch() {
voltage_limit = limit_volt;
// check if the zero found
if(monitor_port){
if(sensor->needsSearch()) monitor_port->println(F("MOT: Error: Not found!"));
else monitor_port->println(F("MOT: Success!"));
if(sensor->needsSearch()) SIMPLEFOC_DEBUG("MOT: Error: Not found!");
else SIMPLEFOC_DEBUG("MOT: Success!");
}
return !sensor->needsSearch();
}
Expand Down Expand Up @@ -309,7 +309,7 @@ void BLDCMotor::loopFOC() {
break;
default:
// no torque control selected
if(monitor_port) monitor_port->println(F("MOT: no torque control selected!"));
SIMPLEFOC_DEBUG("MOT: no torque control selected!");
break;
}

Expand Down
1 change: 1 addition & 0 deletions src/SimpleFOC.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -114,5 +114,6 @@ void loop() {
#include "current_sense/GenericCurrentSense.h"
#include "communication/Commander.h"
#include "communication/StepDirListener.h"
#include "communication/SimpleFOCDebug.h"

#endif
43 changes: 22 additions & 21 deletions src/StepperMotor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,6 @@
#include "StepperMotor.h"
#include "./communication/SimpleFOCDebug.h"


// StepperMotor(int pp)
// - pp - pole pair number
Expand Down Expand Up @@ -27,10 +29,11 @@ void StepperMotor::linkDriver(StepperDriver* _driver) {
void StepperMotor::init() {
if (!driver || !driver->initialized) {
motor_status = FOCMotorStatus::motor_init_failed;
if(monitor_port) monitor_port->println(F("MOT: Init not possible, driver not initialized"));
SIMPLEFOC_DEBUG("MOT: Init not possible, driver not initialized");
return;
}
motor_status = FOCMotorStatus::motor_initializing;
if(monitor_port) monitor_port->println(F("MOT: Init"));
SIMPLEFOC_DEBUG("MOT: Init");

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

_delay(500);
// enable motor
if(monitor_port) monitor_port->println(F("MOT: Enable driver."));
SIMPLEFOC_DEBUG("MOT: Enable driver.");
enable();
_delay(500);

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

if(exit_flag){
if(monitor_port) monitor_port->println(F("MOT: Ready."));
SIMPLEFOC_DEBUG("MOT: Ready.");
motor_status = FOCMotorStatus::motor_ready;
}else{
if(monitor_port) monitor_port->println(F("MOT: Init FOC failed."));
SIMPLEFOC_DEBUG("MOT: Init FOC failed.");
motor_status = FOCMotorStatus::motor_calib_failed;
disable();
}
Expand All @@ -127,7 +130,7 @@ int StepperMotor::initFOC( float zero_electric_offset, Direction _sensor_direct
// Encoder alignment to electrical 0 angle
int StepperMotor::alignSensor() {
int exit_flag = 1; //success
if(monitor_port) monitor_port->println(F("MOT: Align sensor."));
SIMPLEFOC_DEBUG("MOT: Align sensor.");

// if unknown natural direction
if(!_isset(sensor_direction)){
Expand Down Expand Up @@ -158,24 +161,23 @@ int StepperMotor::alignSensor() {
_delay(200);
// determine the direction the sensor moved
if (mid_angle == end_angle) {
if(monitor_port) monitor_port->println(F("MOT: Failed to notice movement"));
SIMPLEFOC_DEBUG("MOT: Failed to notice movement");
return 0; // failed calibration
} else if (mid_angle < end_angle) {
if(monitor_port) monitor_port->println(F("MOT: sensor_direction==CCW"));
SIMPLEFOC_DEBUG("MOT: sensor_direction==CCW");
sensor_direction = Direction::CCW;
} else{
if(monitor_port) monitor_port->println(F("MOT: sensor_direction==CW"));
SIMPLEFOC_DEBUG("MOT: sensor_direction==CW");
sensor_direction = Direction::CW;
}
// check pole pair number
if(monitor_port) monitor_port->print(F("MOT: PP check: "));
float moved = fabs(mid_angle - end_angle);
if( fabs(moved*pole_pairs - _2PI) > 0.5f ) { // 0.5f is arbitrary number it can be lower or higher!
if(monitor_port) monitor_port->print(F("fail - estimated pp:"));
if(monitor_port) monitor_port->println(_2PI/moved,4);
}else if(monitor_port) monitor_port->println(F("OK!"));
SIMPLEFOC_DEBUG("MOT: PP check: fail - estimated pp: ", _2PI/moved);
} else
SIMPLEFOC_DEBUG("MOT: PP check: OK!");

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

// zero electric angle not known
if(!_isset(zero_electric_angle)){
Expand All @@ -190,21 +192,20 @@ int StepperMotor::alignSensor() {
zero_electric_angle = electricalAngle();
_delay(20);
if(monitor_port){
monitor_port->print(F("MOT: Zero elec. angle: "));
monitor_port->println(zero_electric_angle);
SIMPLEFOC_DEBUG("MOT: Zero elec. angle: ", zero_electric_angle);
}
// stop everything
setPhaseVoltage(0, 0, 0);
_delay(200);
}else if(monitor_port) monitor_port->println(F("MOT: Skip offset calib."));
}else SIMPLEFOC_DEBUG("MOT: Skip offset calib.");
return exit_flag;
}

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

if(monitor_port) monitor_port->println(F("MOT: Index search..."));
SIMPLEFOC_DEBUG("MOT: Index search...");
// search the absolute zero with small velocity
float limit_vel = velocity_limit;
float limit_volt = voltage_limit;
Expand All @@ -224,8 +225,8 @@ int StepperMotor::absoluteZeroSearch() {
voltage_limit = limit_volt;
// check if the zero found
if(monitor_port){
if(sensor->needsSearch()) monitor_port->println(F("MOT: Error: Not found!"));
else monitor_port->println(F("MOT: Success!"));
if(sensor->needsSearch()) SIMPLEFOC_DEBUG("MOT: Error: Not found!");
else SIMPLEFOC_DEBUG("MOT: Success!");
}
return !sensor->needsSearch();
}
Expand Down
4 changes: 3 additions & 1 deletion src/common/base_classes/FOCMotor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,5 @@
#include "FOCMotor.h"
#include "../../communication/SimpleFOCDebug.h"

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

// utility function intended to be used with serial plotter to monitor motor variables
Expand Down
Loading