Change references to BLDCDriver to FOCDriver in current sense base class
change setPwm function in FOCDriver to accept 3 args, with a default value for the 3rd Change stepper driver classes to use 3 arg setPwm, ignoring 3rd arg. created InlineCurrentSenseSync class in lib/currentsense. fixed timer bug in calibrate encoder, in main defined a label for serialusb, to make it easier to swap out.
This commit is contained in:
copper280z
254
firmware/lib/currentsense/InlineCurrentSenseSync.cpp
Normal file
254
firmware/lib/currentsense/InlineCurrentSenseSync.cpp
Normal file
@@ -0,0 +1,254 @@
|
||||
#include "InlineCurrentSenseSync.h"
|
||||
#include "communication/SimpleFOCDebug.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)
|
||||
InlineCurrentSenseSync::InlineCurrentSenseSync(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;
|
||||
};
|
||||
|
||||
|
||||
InlineCurrentSenseSync::InlineCurrentSenseSync(float _mVpA, int _pinA, int _pinB, int _pinC){
|
||||
pinA = _pinA;
|
||||
pinB = _pinB;
|
||||
pinC = _pinC;
|
||||
|
||||
volts_to_amps_ratio = 1000.0f / _mVpA; // mV 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
|
||||
int InlineCurrentSenseSync::init(){
|
||||
// if no linked driver its fine in this case
|
||||
// at least for init()
|
||||
void* drv_params = driver ? driver->params : nullptr;
|
||||
// configure ADC variables
|
||||
params = _configureADCInline(drv_params,pinA,pinB,pinC);
|
||||
// if init failed return fail
|
||||
if (params == SIMPLEFOC_CURRENT_SENSE_INIT_FAILED) return 0;
|
||||
_driverSyncLowSide(driver->params, params);
|
||||
// calibrate zero offsets
|
||||
calibrateOffsets();
|
||||
// set the initialized flag
|
||||
initialized = (params!=SIMPLEFOC_CURRENT_SENSE_INIT_FAILED);
|
||||
// return success
|
||||
return 1;
|
||||
}
|
||||
// Function finding zero offsets of the ADC
|
||||
void InlineCurrentSenseSync::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++) {
|
||||
if(_isset(pinA)) offset_ia += _readADCVoltageInline(pinA, params);
|
||||
if(_isset(pinB)) offset_ib += _readADCVoltageInline(pinB, params);
|
||||
if(_isset(pinC)) offset_ic += _readADCVoltageInline(pinC, params);
|
||||
_delay(1);
|
||||
}
|
||||
// calculate the mean offsets
|
||||
if(_isset(pinA)) offset_ia = offset_ia / calibration_rounds;
|
||||
if(_isset(pinB)) 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 InlineCurrentSenseSync::getPhaseCurrents(){
|
||||
PhaseCurrent_s current;
|
||||
current.a = (!_isset(pinA)) ? 0 : (_readADCVoltageInline(pinA, params) - offset_ia)*gain_a;// amps
|
||||
current.b = (!_isset(pinB)) ? 0 : (_readADCVoltageInline(pinB, params) - offset_ib)*gain_b;// amps
|
||||
current.c = (!_isset(pinC)) ? 0 : (_readADCVoltageInline(pinC, params) - offset_ic)*gain_c; // amps
|
||||
return current;
|
||||
}
|
||||
|
||||
// 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 InlineCurrentSenseSync::driverAlign(float voltage){
|
||||
|
||||
int exit_flag = 1;
|
||||
if(skip_align) return exit_flag;
|
||||
|
||||
if (driver==nullptr) {
|
||||
SIMPLEFOC_DEBUG("CUR: No driver linked!");
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (!initialized) return 0;
|
||||
|
||||
if(_isset(pinA)){
|
||||
// set phase A active and phases B and C down
|
||||
driver->setPwm(voltage, 0, 0);
|
||||
_delay(2000);
|
||||
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 = c.b ? fabs(c.a / c.b) : 0;
|
||||
float ac_ratio = c.c ? fabs(c.a / c.c) : 0;
|
||||
if(_isset(pinB) && ab_ratio > 1.5f ){ // should be ~2
|
||||
gain_a *= _sign(c.a);
|
||||
}else if(_isset(pinC) && ac_ratio > 1.5f ){ // should be ~2
|
||||
gain_a *= _sign(c.a);
|
||||
}else if(_isset(pinB) && ab_ratio < 0.7f ){ // should be ~0.5
|
||||
// switch phase A and B
|
||||
int tmp_pinA = pinA;
|
||||
pinA = pinB;
|
||||
pinB = tmp_pinA;
|
||||
float tmp_offsetA = offset_ia;
|
||||
offset_ia = offset_ib;
|
||||
offset_ib = tmp_offsetA;
|
||||
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;
|
||||
float tmp_offsetA = offset_ia;
|
||||
offset_ia = offset_ic;
|
||||
offset_ic = tmp_offsetA;
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
if(_isset(pinB)){
|
||||
// set phase B active and phases A and C down
|
||||
driver->setPwm(0, voltage, 0);
|
||||
_delay(200);
|
||||
PhaseCurrent_s c = getPhaseCurrents();
|
||||
// read the current 50 times
|
||||
for (int i = 0; i < 100; i++) {
|
||||
PhaseCurrent_s c1 = getPhaseCurrents();
|
||||
c.a = c.a*0.6 + 0.4f*c1.a;
|
||||
c.b = c.b*0.6 + 0.4f*c1.b;
|
||||
c.c = c.c*0.6 + 0.4f*c1.c;
|
||||
_delay(3);
|
||||
}
|
||||
driver->setPwm(0, 0, 0);
|
||||
float ba_ratio = c.a ? fabs(c.b / c.a) : 0;
|
||||
float bc_ratio = c.c ? fabs(c.b / c.c) : 0;
|
||||
if(_isset(pinA) && ba_ratio > 1.5f ){ // should be ~2
|
||||
gain_b *= _sign(c.b);
|
||||
}else if(_isset(pinC) && bc_ratio > 1.5f ){ // should be ~2
|
||||
gain_b *= _sign(c.b);
|
||||
}else if(_isset(pinA) && ba_ratio < 0.7f ){ // it should be ~0.5
|
||||
// switch phase A and B
|
||||
int tmp_pinB = pinB;
|
||||
pinB = pinA;
|
||||
pinA = tmp_pinB;
|
||||
float tmp_offsetB = offset_ib;
|
||||
offset_ib = offset_ia;
|
||||
offset_ia = tmp_offsetB;
|
||||
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;
|
||||
float tmp_offsetB = offset_ib;
|
||||
offset_ib = offset_ic;
|
||||
offset_ic = tmp_offsetB;
|
||||
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 C active and phases A and B down
|
||||
driver->setPwm(0, 0, voltage);
|
||||
_delay(200);
|
||||
PhaseCurrent_s c = getPhaseCurrents();
|
||||
// read the adc voltage 500 times ( arbitrary number )
|
||||
for (int i = 0; i < 100; i++) {
|
||||
PhaseCurrent_s c1 = getPhaseCurrents();
|
||||
c.a = c.a*0.6 + 0.4f*c1.a;
|
||||
c.b = c.b*0.6 + 0.4f*c1.b;
|
||||
c.c = c.c*0.6 + 0.4f*c1.c;
|
||||
_delay(3);
|
||||
}
|
||||
driver->setPwm(0, 0, 0);
|
||||
float ca_ratio = c.a ? fabs(c.c / c.a) : 0;
|
||||
float cb_ratio = c.b ? fabs(c.c / c.b) : 0;
|
||||
if(_isset(pinA) && ca_ratio > 1.5f ){ // should be ~2
|
||||
gain_c *= _sign(c.c);
|
||||
}else if(_isset(pinB) && cb_ratio > 1.5f ){ // should be ~2
|
||||
gain_c *= _sign(c.c);
|
||||
}else if(_isset(pinA) && ca_ratio < 0.7f ){ // it should be ~0.5
|
||||
// switch phase A and C
|
||||
int tmp_pinC = pinC;
|
||||
pinC = pinA;
|
||||
pinA = tmp_pinC;
|
||||
float tmp_offsetC = offset_ic;
|
||||
offset_ic = offset_ia;
|
||||
offset_ia = tmp_offsetC;
|
||||
gain_c *= _sign(c.a);
|
||||
exit_flag = 2; // signal that pins have been switched
|
||||
}else if(_isset(pinB) && cb_ratio < 0.7f ){ // should be ~0.5
|
||||
// switch phase B and C
|
||||
int tmp_pinC = pinC;
|
||||
pinC = pinB;
|
||||
pinB = tmp_pinC;
|
||||
float tmp_offsetC = offset_ic;
|
||||
offset_ic = offset_ib;
|
||||
offset_ib = tmp_offsetC;
|
||||
gain_c *= _sign(c.b);
|
||||
exit_flag = 2; // signal that pins have been switched
|
||||
}else{
|
||||
// error in current sense - phase either not measured or bad connection
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
75
firmware/lib/currentsense/InlineCurrentSenseSync.h
Normal file
75
firmware/lib/currentsense/InlineCurrentSenseSync.h
Normal file
@@ -0,0 +1,75 @@
|
||||
#ifndef INLINE_CS_SYNC_LIB_H
|
||||
#define INLINE_CS_SYNC_LIB_H
|
||||
|
||||
#include "Arduino.h"
|
||||
#include "SimpleFOC.h"
|
||||
#include "common/foc_utils.h"
|
||||
#include "common/time_utils.h"
|
||||
#include "common/defaults.h"
|
||||
#include "common/base_classes/CurrentSense.h"
|
||||
#include "common/lowpass_filter.h"
|
||||
// #include "current_sense/hardware_api.h"
|
||||
#include "utils.h"
|
||||
|
||||
|
||||
class InlineCurrentSenseSync: public CurrentSense{
|
||||
public:
|
||||
/**
|
||||
InlineCurrentSense class constructor
|
||||
@param shunt_resistor shunt resistor value
|
||||
@param gain current-sense op-amp gain
|
||||
@param phA A phase adc pin
|
||||
@param phB B phase adc pin
|
||||
@param phC C phase adc pin (optional)
|
||||
*/
|
||||
InlineCurrentSenseSync(float shunt_resistor, float gain, int pinA, int pinB, int pinC = NOT_SET);
|
||||
/**
|
||||
InlineCurrentSense class constructor
|
||||
@param mVpA mV per Amp ratio
|
||||
@param phA A phase adc pin
|
||||
@param phB B phase adc pin
|
||||
@param phC C phase adc pin (optional)
|
||||
*/
|
||||
InlineCurrentSenseSync(float mVpA, int pinA, int pinB, int pinC = NOT_SET);
|
||||
|
||||
// CurrentSense interface implementing functions
|
||||
int init() override;
|
||||
PhaseCurrent_s getPhaseCurrents() override;
|
||||
int driverAlign(float align_voltage) override;
|
||||
|
||||
// ADC measuremnet gain for each phase
|
||||
// support for different gains for different phases of more commonly - inverted phase currents
|
||||
// this should be automated later
|
||||
float gain_a; //!< phase A gain
|
||||
float gain_b; //!< phase B gain
|
||||
float gain_c; //!< phase C gain
|
||||
|
||||
// // per phase low pass fileters
|
||||
// LowPassFilter lpf_a{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current A low pass filter
|
||||
// LowPassFilter lpf_b{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current B low pass filter
|
||||
// LowPassFilter lpf_c{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current C low pass filter
|
||||
|
||||
float offset_ia; //!< zero current A voltage value (center of the adc reading)
|
||||
float offset_ib; //!< zero current B voltage value (center of the adc reading)
|
||||
float offset_ic; //!< zero current C voltage value (center of the adc reading)
|
||||
|
||||
private:
|
||||
|
||||
// hardware variables
|
||||
int pinA; //!< pin A analog pin for current measurement
|
||||
int pinB; //!< pin B analog pin for current measurement
|
||||
int pinC; //!< pin C analog pin for current measurement
|
||||
|
||||
// gain variables
|
||||
float shunt_resistor; //!< Shunt resistor value
|
||||
float amp_gain; //!< amp gain value
|
||||
float volts_to_amps_ratio; //!< Volts to amps ratio
|
||||
|
||||
/**
|
||||
* Function finding zero offsets of the ADC
|
||||
*/
|
||||
void calibrateOffsets();
|
||||
|
||||
};
|
||||
|
||||
#endif
|
||||
Reference in New Issue
Block a user