Signed-off-by: myperworld <myperworld@126.com>

v1.1/Shibo-main-TOMMY/lib/Simple_FOC/src/current_sense/GenericCurrentSense.cpp

@@ -0,0 +1,162 @@
+#include "GenericCurrentSense.h"
+
+// GenericCurrentSense constructor
+GenericCurrentSense::GenericCurrentSense(PhaseCurrent_s (*readCallback)(), void (*initCallback)()){
+  // if function provided add it to the 
+  if(readCallback != nullptr) this->readCallback = readCallback;
+  if(initCallback != nullptr) this->initCallback = initCallback;
+}
+
+// Inline sensor init function
+void GenericCurrentSense::init(){
+    // configure ADC variables
+    if(initCallback != nullptr) initCallback();
+    // calibrate zero offsets
+    calibrateOffsets();
+}
+// Function finding zero offsets of the ADC
+void GenericCurrentSense::calibrateOffsets(){
+    const int calibration_rounds = 1000;
+
+    // find adc offset = zero current voltage
+    offset_ia = 0;
+    offset_ib = 0;
+    offset_ic = 0;
+    // read the adc voltage 1000 times ( arbitrary number )
+    for (int i = 0; i < calibration_rounds; i++) {
+        PhaseCurrent_s current = readCallback();
+        offset_ia += current.a;
+        offset_ib += current.b;
+        offset_ic += current.c;
+        _delay(1);
+    }
+    // calculate the mean offsets
+    offset_ia = offset_ia / calibration_rounds;
+    offset_ib = offset_ib / calibration_rounds;
+    offset_ic = offset_ic / calibration_rounds;
+}
+
+// read all three phase currents (if possible 2 or 3)
+PhaseCurrent_s GenericCurrentSense::getPhaseCurrents(){
+    PhaseCurrent_s current = readCallback();
+    current.a = (current.a - offset_ia);// amps
+    current.b = (current.a - offset_ib);// amps
+    current.c = (current.a - offset_ic); // amps
+    return current;
+}
+
+// Function synchronizing current sense with motor driver.
+// for in-line sensig no such thing is necessary
+int GenericCurrentSense::driverSync(BLDCDriver *driver){
+    return 1;
+}
+
+// Function aligning the current sense with motor driver
+// if all pins are connected well none of this is really necessary! - can be avoided
+// returns flag
+// 0 - fail
+// 1 - success and nothing changed
+// 2 - success but pins reconfigured
+// 3 - success but gains inverted
+// 4 - success but pins reconfigured and gains inverted
+int GenericCurrentSense::driverAlign(BLDCDriver *driver, float voltage){
+    int exit_flag = 1;
+    if(skip_align) return exit_flag;
+
+    // // set phase A active and phases B and C down
+    // driver->setPwm(voltage, 0, 0);
+    // _delay(200);
+    // PhaseCurrent_s c = getPhaseCurrents();
+    // // read the current 100 times ( arbitrary number )
+    // for (int i = 0; i < 100; i++) {
+    //     PhaseCurrent_s c1 = getPhaseCurrents();
+    //     c.a = c.a*0.6f + 0.4f*c1.a;
+    //     c.b = c.b*0.6f + 0.4f*c1.b;
+    //     c.c = c.c*0.6f + 0.4f*c1.c;
+    //     _delay(3);
+    // }
+    // driver->setPwm(0, 0, 0);
+    // // align phase A
+    // float ab_ratio = fabs(c.a / c.b);
+    // float ac_ratio = c.c ? fabs(c.a / c.c) : 0;
+    // if( ab_ratio > 1.5f ){ // should be ~2
+    //     gain_a *= _sign(c.a);
+    // }else if( ab_ratio < 0.7f ){ // should be ~0.5
+    //     // switch phase A and B
+    //     int tmp_pinA = pinA;
+    //     pinA = pinB;
+    //     pinB = tmp_pinA;
+    //     gain_a *= _sign(c.b);
+    //     exit_flag = 2; // signal that pins have been switched
+    // }else if(_isset(pinC) &&  ac_ratio < 0.7f ){ // should be ~0.5
+    //     // switch phase A and C
+    //     int tmp_pinA = pinA;
+    //     pinA = pinC;
+    //     pinC= tmp_pinA;
+    //     gain_a *= _sign(c.c);
+    //     exit_flag = 2;// signal that pins have been switched
+    // }else{
+    //     // error in current sense - phase either not measured or bad connection
+    //     return 0;
+    // }
+
+    // // set phase B active and phases A and C down
+    // driver->setPwm(0, voltage, 0);
+    // _delay(200);
+    // c = getPhaseCurrents();
+    // // read the current 50 times
+    // for (int i = 0; i < 100; i++) {
+    //     PhaseCurrent_s c1 = getPhaseCurrents();
+    //     c.a = c.a*0.6f + 0.4f*c1.a;
+    //     c.b = c.b*0.6f + 0.4f*c1.b;
+    //     c.c = c.c*0.6f + 0.4f*c1.c;
+    //     _delay(3);
+    // }
+    // driver->setPwm(0, 0, 0);
+    // float ba_ratio = fabs(c.b/c.a);
+    // float bc_ratio = c.c ? fabs(c.b / c.c) : 0;
+    //  if( ba_ratio > 1.5f ){ // should be ~2
+    //     gain_b *= _sign(c.b);
+    // }else if( ba_ratio < 0.7f ){ // it should be ~0.5
+    //     // switch phase A and B
+    //     int tmp_pinB = pinB;
+    //     pinB = pinA;
+    //     pinA = tmp_pinB;
+    //     gain_b *= _sign(c.a);
+    //     exit_flag = 2; // signal that pins have been switched
+    // }else if(_isset(pinC) && bc_ratio < 0.7f ){ // should be ~0.5
+    //     // switch phase A and C
+    //     int tmp_pinB = pinB;
+    //     pinB = pinC;
+    //     pinC = tmp_pinB;
+    //     gain_b *= _sign(c.c);
+    //     exit_flag = 2; // signal that pins have been switched
+    // }else{
+    //     // error in current sense - phase either not measured or bad connection
+    //     return 0;
+    // }
+
+    // // if phase C measured
+    // if(_isset(pinC)){
+    //     // set phase B active and phases A and C down
+    //     driver->setPwm(0, 0, voltage);
+    //     _delay(200);
+    //     c = getPhaseCurrents();
+    //     // read the adc voltage 500 times ( arbitrary number )
+    //     for (int i = 0; i < 50; i++) {
+    //         PhaseCurrent_s c1 = getPhaseCurrents();
+    //         c.c = (c.c+c1.c)/50.0f;
+    //     }
+    //     driver->setPwm(0, 0, 0);
+    //     gain_c *= _sign(c.c);
+    // }
+
+    // if(gain_a < 0 || gain_b < 0 || gain_c < 0) exit_flag +=2;
+    // exit flag is either
+    // 0 - fail
+    // 1 - success and nothing changed
+    // 2 - success but pins reconfigured
+    // 3 - success but gains inverted
+    // 4 - success but pins reconfigured and gains inverted
+    return exit_flag;
+}