diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index acc5b0fe..792381d5 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -202,7 +202,7 @@ int BLDCMotor::alignCurrentSense() { SIMPLEFOC_DEBUG("MOT: Align current sense."); // align current sense and the driver - exit_flag = current_sense->driverAlign(voltage_sensor_align); + exit_flag = current_sense->driverAlign(voltage_sensor_align, modulation_centered); if(!exit_flag){ // error in current sense - phase either not measured or bad connection SIMPLEFOC_DEBUG("MOT: Align error!"); diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index 38e8999f..5db9afa2 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -159,7 +159,7 @@ int StepperMotor::alignCurrentSense() { SIMPLEFOC_DEBUG("MOT: Align current sense."); // align current sense and the driver - exit_flag = current_sense->driverAlign(voltage_sensor_align); + exit_flag = current_sense->driverAlign(voltage_sensor_align, modulation_centered); if(!exit_flag){ // error in current sense - phase either not measured or bad connection SIMPLEFOC_DEBUG("MOT: Align error!"); diff --git a/src/common/base_classes/CurrentSense.cpp b/src/common/base_classes/CurrentSense.cpp index 5ef906e0..84bc3547 100644 --- a/src/common/base_classes/CurrentSense.cpp +++ b/src/common/base_classes/CurrentSense.cpp @@ -133,7 +133,7 @@ void CurrentSense::disable(){ // 3 - success but gains inverted // 4 - success but pins reconfigured and gains inverted // IMPORTANT, this function can be overriden in the child class -int CurrentSense::driverAlign(float voltage){ +int CurrentSense::driverAlign(float voltage, bool modulation_centered){ int exit_flag = 1; if(skip_align) return exit_flag; @@ -142,9 +142,9 @@ int CurrentSense::driverAlign(float voltage){ // check if stepper or BLDC if(driver_type == DriverType::Stepper) - return alignStepperDriver(voltage, (StepperDriver*)driver); + return alignStepperDriver(voltage, (StepperDriver*)driver, modulation_centered); else - return alignBLDCDriver(voltage, (BLDCDriver*)driver); + return alignBLDCDriver(voltage, (BLDCDriver*)driver, modulation_centered); } @@ -172,22 +172,23 @@ PhaseCurrent_s CurrentSense::readAverageCurrents(int N) { // 2 - success but pins reconfigured // 3 - success but gains inverted // 4 - success but pins reconfigured and gains inverted -int CurrentSense::alignBLDCDriver(float voltage, BLDCDriver* bldc_driver){ +int CurrentSense::alignBLDCDriver(float voltage, BLDCDriver* bldc_driver, bool modulation_centered){ bool phases_switched = 0; bool phases_inverted = 0; - float center = driver->voltage_limit/2.0; + float zero = 0; + if(modulation_centered) zero = driver->voltage_limit/2.0; // set phase A active and phases B and C down // 300 ms of ramping for(int i=0; i < 100; i++){ - bldc_driver->setPwm(voltage/100.0*((float)i) , 0, 0); + bldc_driver->setPwm(voltage/100.0*((float)i)+zero , zero, zero); _delay(3); } _delay(500); PhaseCurrent_s c_a = readAverageCurrents(); - bldc_driver->setPwm(0, 0, 0); + bldc_driver->setPwm(zero, zero, zero); // check if currents are to low (lower than 100mA) // TODO calculate the 100mA threshold from the ADC resolution // if yes throw an error and return 0 @@ -293,12 +294,12 @@ int CurrentSense::alignBLDCDriver(float voltage, BLDCDriver* bldc_driver){ // set phase B active and phases A and C down // 300 ms of ramping for(int i=0; i < 100; i++){ - bldc_driver->setPwm(0, voltage/100.0*((float)i), 0); + bldc_driver->setPwm(zero, voltage/100.0*((float)i)+zero, zero); _delay(3); } _delay(500); PhaseCurrent_s c_b = readAverageCurrents(); - bldc_driver->setPwm(0, 0, 0); + bldc_driver->setPwm(zero, zero, zero); // check the phase B // find the highest magnitude in c_b @@ -390,10 +391,13 @@ int CurrentSense::alignBLDCDriver(float voltage, BLDCDriver* bldc_driver){ // 2 - success but pins reconfigured // 3 - success but gains inverted // 4 - success but pins reconfigured and gains inverted -int CurrentSense::alignStepperDriver(float voltage, StepperDriver* stepper_driver){ +int CurrentSense::alignStepperDriver(float voltage, StepperDriver* stepper_driver, bool modulation_centered){ bool phases_switched = 0; bool phases_inverted = 0; + + float zero = 0; + if(modulation_centered) zero = driver->voltage_limit/2.0; if(!_isset(pinA) || !_isset(pinB)){ SIMPLEFOC_DEBUG("CS: Pins A & B not specified!"); @@ -403,13 +407,13 @@ int CurrentSense::alignStepperDriver(float voltage, StepperDriver* stepper_drive // set phase A active and phases B down // ramp 300ms for(int i=0; i < 100; i++){ - stepper_driver->setPwm(voltage/100.0*((float)i), 0); + stepper_driver->setPwm(voltage/100.0*((float)i), zero); _delay(3); } _delay(500); PhaseCurrent_s c = readAverageCurrents(); // disable the phases - stepper_driver->setPwm(0, 0); + stepper_driver->setPwm(zero, zero); if (fabs(c.a) < 0.1f && fabs(c.b) < 0.1f ){ SIMPLEFOC_DEBUG("CS: Err too low current!"); return 0; // measurement current too low @@ -438,12 +442,12 @@ int CurrentSense::alignStepperDriver(float voltage, StepperDriver* stepper_drive // set phase B active and phases A down // ramp 300ms for(int i=0; i < 100; i++){ - stepper_driver->setPwm(0, voltage/100.0*((float)i)); + stepper_driver->setPwm(zero, voltage/100.0*((float)i)+zero); _delay(3); } _delay(500); c = readAverageCurrents(); - stepper_driver->setPwm(0, 0); + stepper_driver->setPwm(zero, zero); // phase B should be aligned // 1) we just need to verify that it has been measured diff --git a/src/common/base_classes/CurrentSense.h b/src/common/base_classes/CurrentSense.h index c9c79650..d3f7f8ed 100644 --- a/src/common/base_classes/CurrentSense.h +++ b/src/common/base_classes/CurrentSense.h @@ -68,7 +68,7 @@ class CurrentSense{ * * IMPORTANT: Default implementation provided in the CurrentSense class, but can be overriden in the child classes */ - virtual int driverAlign(float align_voltage); + virtual int driverAlign(float align_voltage, bool modulation_centered = false); /** * Function rading the phase currents a, b and c @@ -130,11 +130,11 @@ class CurrentSense{ /** * Function used to align the current sense with the BLDC motor driver */ - int alignBLDCDriver(float align_voltage, BLDCDriver* driver); + int alignBLDCDriver(float align_voltage, BLDCDriver* driver, bool modulation_centered); /** * Function used to align the current sense with the Stepper motor driver */ - int alignStepperDriver(float align_voltage, StepperDriver* driver); + int alignStepperDriver(float align_voltage, StepperDriver* driver, bool modulation_centered); /** * Function used to read the average current values over N samples */ diff --git a/src/current_sense/GenericCurrentSense.cpp b/src/current_sense/GenericCurrentSense.cpp index 9d90f0ca..54c4f12e 100644 --- a/src/current_sense/GenericCurrentSense.cpp +++ b/src/current_sense/GenericCurrentSense.cpp @@ -54,7 +54,7 @@ PhaseCurrent_s GenericCurrentSense::getPhaseCurrents(){ // returns flag // 0 - fail // 1 - success and nothing changed -int GenericCurrentSense::driverAlign(float voltage){ +int GenericCurrentSense::driverAlign(float voltage, bool modulation_centered){ _UNUSED(voltage) ; // remove unused parameter warning int exit_flag = 1; if(skip_align) return exit_flag; diff --git a/src/current_sense/GenericCurrentSense.h b/src/current_sense/GenericCurrentSense.h index a63df49d..02bf8fa9 100644 --- a/src/current_sense/GenericCurrentSense.h +++ b/src/current_sense/GenericCurrentSense.h @@ -20,7 +20,7 @@ class GenericCurrentSense: public CurrentSense{ // CurrentSense interface implementing functions int init() override; PhaseCurrent_s getPhaseCurrents() override; - int driverAlign(float align_voltage) override; + int driverAlign(float align_voltage, bool modulation_centered) override; PhaseCurrent_s (*readCallback)() = nullptr; //!< function pointer to sensor reading