优化了平衡，尤其对于向后倾倒的几种极限情况进行优化

Shibo-main-TOMMY/lib/Simple_FOC/src/current_sense/InlineCurrentSense.cpp

@@ -0,0 +1,172 @@
+#include "InlineCurrentSense.h"
+// InlineCurrentSensor constructor
+//  - shunt_resistor  - shunt resistor value
+//  - gain  - current-sense op-amp gain
+//  - phA   - A phase adc pin
+//  - phB   - B phase adc pin
+//  - phC   - C phase adc pin (optional)
+InlineCurrentSense::InlineCurrentSense(float _shunt_resistor, float _gain, int _pinA, int _pinB, int _pinC){
+    pinA = _pinA;
+    pinB = _pinB;
+    pinC = _pinC;
+
+    shunt_resistor = _shunt_resistor;
+    amp_gain  = _gain;
+    volts_to_amps_ratio = 1.0f /_shunt_resistor / _gain; // volts to amps
+    // gains for each phase
+    gain_a = volts_to_amps_ratio;
+    gain_b = volts_to_amps_ratio;
+    gain_c = volts_to_amps_ratio;
+}
+
+// Inline sensor init function
+void InlineCurrentSense::init(){
+    // configure ADC variables
+    _configureADCInline(pinA,pinB,pinC);
+    // calibrate zero offsets
+    calibrateOffsets();
+}
+// Function finding zero offsets of the ADC
+void InlineCurrentSense::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++) {
+        offset_ia += _readADCVoltageInline(pinA);
+        offset_ib += _readADCVoltageInline(pinB);
+        if(_isset(pinC)) offset_ic += _readADCVoltageInline(pinC);
+        _delay(1);
+    }
+    // calculate the mean offsets
+    offset_ia = offset_ia / calibration_rounds;
+    offset_ib = offset_ib / calibration_rounds;
+    if(_isset(pinC)) offset_ic = offset_ic / calibration_rounds;
+}
+
+// read all three phase currents (if possible 2 or 3)
+PhaseCurrent_s InlineCurrentSense::getPhaseCurrents(){
+    PhaseCurrent_s current;
+    current.a = (_readADCVoltageInline(pinA) - offset_ia)*gain_a;// amps
+    current.b = (_readADCVoltageInline(pinB) - offset_ib)*gain_b;// amps
+    current.c = (!_isset(pinC)) ? 0 : (_readADCVoltageInline(pinC) - offset_ic)*gain_c; // amps
+    return current;
+}
+// Function synchronizing current sense with motor driver.
+// for in-line sensig no such thing is necessary
+int InlineCurrentSense::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 InlineCurrentSense::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;
+}