feactoring and added ramping for current sense align

Author: askuric

README.md

@@ -13,7 +13,8 @@
 ![GitHub commits since tagged version](https://img.shields.io/github/commits-since/simplefoc/arduino-foc/latest/dev)
 ![GitHub commit activity (branch)](https://img.shields.io/github/commit-activity/m/simplefoc/arduino-foc/dev)
 
-[![arduino-library-badge](https://www.ardu-badge.com/badge/Simple%20FOC.svg?)](https://www.ardu-badge.com/badge/Simple%20FOC.svg)
+[![arduino-library-badge](https://ardubadge.simplefoc.com?lib=Simple%20FOC)](https://www.ardu-badge.com/badge/Simple%20FOC.svg)
+[![PlatformIO Registry](https://badges.registry.platformio.org/packages/askuric/library/Simple%20FOC.svg)](https://registry.platformio.org/libraries/askuric/Simple%20FOC)
 [![License: MIT](https://img.shields.io/badge/License-MIT-yellow.svg)](https://opensource.org/licenses/MIT)
 [![status](https://joss.theoj.org/papers/4382445f249e064e9f0a7f6c1bb06b1d/status.svg)](https://joss.theoj.org/papers/4382445f249e064e9f0a7f6c1bb06b1d)
 

src/common/base_classes/CurrentSense.cpp

@@ -177,11 +177,17 @@ int CurrentSense::alignBLDCDriver(float voltage, BLDCDriver* bldc_driver){
     bool phases_switched = 0;
     bool phases_inverted = 0;
 
-    
+    float center = driver->voltage_limit/2.0;
+
     // set phase A active and phases B and C down
-    bldc_driver->setPwm(voltage, 0, 0);
+    // 300 ms of ramping
+    for(int i=0; i < 100; i++){
+        bldc_driver->setPwm(voltage/100.0*((float)i) , 0, 0);
+        _delay(3);
+    }
     _delay(500);
     PhaseCurrent_s c_a = readAverageCurrents();
+    bldc_driver->setPwm(0, 0, 0);
     // 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
@@ -191,6 +197,7 @@ int CurrentSense::alignBLDCDriver(float voltage, BLDCDriver* bldc_driver){
             SIMPLEFOC_DEBUG("CS: Err too low current, rise voltage!");
             return 0; // measurement current too low
     }
+
 
     // now we have to determine 
     // 1) which pin correspond to which phase of the bldc driver
@@ -284,7 +291,11 @@ int CurrentSense::alignBLDCDriver(float voltage, BLDCDriver* bldc_driver){
 
 
     // set phase B active and phases A and C down
-    bldc_driver->setPwm(0, voltage, 0);
+    // 300 ms of ramping
+    for(int i=0; i < 100; i++){
+        bldc_driver->setPwm(0, voltage/100.0*((float)i), 0);
+        _delay(3);
+    }
     _delay(500);
     PhaseCurrent_s c_b = readAverageCurrents();
     bldc_driver->setPwm(0, 0, 0);
@@ -384,53 +395,67 @@ int CurrentSense::alignStepperDriver(float voltage, StepperDriver* stepper_drive
     bool phases_switched = 0;
     bool phases_inverted = 0;
 
-    if(_isset(pinA)){
-        // set phase A active to high and B to low
-        stepper_driver->setPwm(voltage, 0);
-        _delay(500);
-        PhaseCurrent_s c = readAverageCurrents();
-        // disable the phases
-        stepper_driver->setPwm(0, 0);        
-        if (fabs(c.a) < 0.1f && fabs(c.b) < 0.1f ){
-            SIMPLEFOC_DEBUG("CS: Err too low current!");
-            return 0; // measurement current too low
-        }
-        // align phase A
-        // check if measured current a is positive and invert if not
-        // check if current b is around zero and if its not 
-        // check if current a is near zero and if it is invert them
-        if (fabs(c.a) < fabs(c.b)){
-            SIMPLEFOC_DEBUG("CS: Switch A-B");
-            // switch phase A and B
-            _swap(pinA, pinB);
-            _swap(offset_ia, offset_ib);
-            _swap(gain_a, gain_b);
-            gain_a *= _sign(c.b);
-            phases_switched = true; // signal that pins have been switched
-        }else if (c.a < 0){
-            SIMPLEFOC_DEBUG("CS: Inv A");
-            gain_a *= -1;
-            phases_inverted = true; // signal that pins have been inverted
-        }
+    if(!_isset(pinA) || !_isset(pinB)){
+        SIMPLEFOC_DEBUG("CS: Pins A & B not specified!");
+        return 0;
     }
 
-    if(_isset(pinB)){
-        // set phase B active and phases A and C down
-        stepper_driver->setPwm(voltage, 0);
-        _delay(500);
-        PhaseCurrent_s c = readAverageCurrents();
-        stepper_driver->setPwm(0, 0);
-        if (fabs(c.a) < 0.1f && fabs(c.b) < 0.1f ){
-            SIMPLEFOC_DEBUG("CS: Err too low current!");
-            return 0; // measurement current too low
-        }
-        // align phase A
-        // check if measured current a is positive and invert if not
-        if (c.b < 0){
-            SIMPLEFOC_DEBUG("CS: Inv B");
-            gain_b *= -1;
-            phases_inverted = true; // signal that pins have been inverted
-        }
+    // 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);
+        _delay(3);
+    }
+    _delay(500);
+    PhaseCurrent_s c = readAverageCurrents();
+    // disable the phases
+    stepper_driver->setPwm(0, 0);        
+    if (fabs(c.a) < 0.1f && fabs(c.b) < 0.1f ){
+        SIMPLEFOC_DEBUG("CS: Err too low current!");
+        return 0; // measurement current too low
+    }
+    // align phase A
+    // 1) only one phase can be measured so we first measure which ADC pin corresponds 
+    // to the phase A by comparing the magnitude
+    if (fabs(c.a) < fabs(c.b)){
+        SIMPLEFOC_DEBUG("CS: Switch A-B");
+        // switch phase A and B
+        _swap(pinA, pinB);
+        _swap(offset_ia, offset_ib);
+        _swap(gain_a, gain_b);
+        phases_switched = true; // signal that pins have been switched
+    }
+    // 2) check if measured current a is positive and invert if not
+    if (c.a < 0){
+        SIMPLEFOC_DEBUG("CS: Inv A");
+        gain_a *= -1;
+        phases_inverted = true; // signal that pins have been inverted
+    }
+
+    // at this point the driver's phase A is aligned with the ADC pinA
+    // and the pin B should be the phase B
+
+    // 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));
+        _delay(3);
+    }
+    _delay(500);
+    c = readAverageCurrents();
+    stepper_driver->setPwm(0, 0);
+
+    // phase B should be aligned
+    // 1) we just need to verify that it has been measured
+    if (fabs(c.a) < 0.1f && fabs(c.b) < 0.1f ){
+        SIMPLEFOC_DEBUG("CS: Err too low current!");
+        return 0; // measurement current too low
+    }
+    // 2) check if measured current a is positive and invert if not
+    if (c.b < 0){
+        SIMPLEFOC_DEBUG("CS: Inv B");
+        gain_b *= -1;
+        phases_inverted = true; // signal that pins have been inverted
     }
 
     // construct the return flag

src/common/foc_utils.h

@@ -35,7 +35,7 @@
 #define _HIGH_IMPEDANCE 0
 #define _HIGH_Z _HIGH_IMPEDANCE
 #define _ACTIVE 1
-#define _NC (NOT_SET)
+#define _NC ((int) NOT_SET)
 
 #define MIN_ANGLE_DETECT_MOVEMENT (_2PI/101.0f)