matei/lemon-pepper-stepper
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Change references to BLDCDriver to FOCDriver in current sense base class

Browse Source 
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
2023-11-10 21:49:39 -05:00
parent deea94b076
commit 5125602875
11 changed files with 390 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
firmware/lib/Arduino-FOC/src/common/base_classes/CurrentSense.cpp
View File

@@ -110,12 +110,12 @@ DQCurrent_s CurrentSense::getFOCCurrents(float angle_el)
/**
Driver linking to the current sense
*/
void CurrentSense::linkDriver(BLDCDriver *_driver)
void CurrentSense::linkDriver(FOCDriver *_driver)
{
driver = _driver;
}
void CurrentSense::linkDriver(StepperDriver *_driver)
{
driver = _driver;
}
// void CurrentSense::linkDriver(StepperDriver *_driver)
// {
// driver = _driver;
// }

6
firmware/lib/Arduino-FOC/src/common/base_classes/CurrentSense.h
View File

@@ -24,8 +24,8 @@ class CurrentSense{
* Linking the current sense with the motor driver
* Only necessary if synchronisation in between the two is required
*/
void linkDriver(BLDCDriver *driver);
void linkDriver(StepperDriver *driver);
void linkDriver(FOCDriver *driver);
// void linkDriver(StepperDriver *driver);
// variables
bool skip_align = false; //!< variable signaling that the phase current direction should be verified during initFOC()
@@ -72,4 +72,4 @@ class CurrentSense{
DQCurrent_s getFOCCurrents(float angle_el);
};
#endif
#endif

10
firmware/lib/Arduino-FOC/src/common/base_classes/FOCDriver.h
View File

@@ -1,7 +1,8 @@
#ifndef FOCMOTOR_H
#define FOTMOTOR_H
#ifndef FOCDRIVER_H
#define FOCDRIVER_H
#include "Arduino.h"
#include "../foc_utils.h"
class FOCDriver{
public:
@@ -11,6 +12,9 @@ class FOCDriver{
virtual void disable() = 0;
virtual void setPwm(float a, float b, float c);
// virtual void setPwm(uint8_t a, uint8_t b);
long pwm_frequency;
float voltage_power_supply;
float voltage_limit;
@@ -18,4 +22,4 @@ class FOCDriver{
void* params = 0;
};
#endif
#endif

4
firmware/lib/Arduino-FOC/src/common/base_classes/StepperDriver.h
View File

@@ -12,7 +12,7 @@ class StepperDriver: public FOCDriver{
* @param Ua phase A voltage
* @param Ub phase B voltage
*/
virtual void setPwm(float Ua, float Ub) = 0;
virtual void setPwm(float Ua, float Ub, float Uc=NOT_SET) = 0;
};
#endif
#endif

4
firmware/lib/Arduino-FOC/src/drivers/StepperDriver2PWM.cpp
View File

@@ -86,7 +86,7 @@ int StepperDriver2PWM::init() {
// Set voltage to the pwm pin
void StepperDriver2PWM::setPwm(float Ua, float Ub) {
void StepperDriver2PWM::setPwm(float Ua, float Ub, float Uc) {
float duty_cycle1(0.0f),duty_cycle2(0.0f);
// limit the voltage in driver
Ua = _constrain(Ua, -voltage_limit, voltage_limit);
@@ -104,4 +104,4 @@ void StepperDriver2PWM::setPwm(float Ua, float Ub) {
// write to hardware
_writeDutyCycle2PWM(duty_cycle1, duty_cycle2, params);
}
}

2
firmware/lib/Arduino-FOC/src/drivers/StepperDriver2PWM.h
View File

@@ -58,7 +58,7 @@ class StepperDriver2PWM: public StepperDriver
* @param Ua phase A voltage
* @param Ub phase B voltage
*/
void setPwm(float Ua, float Ub) override;
void setPwm(float Ua, float Ub, float NOT_USED=NOT_SET) override;
private:

5
firmware/lib/Arduino-FOC/src/drivers/StepperDriver4PWM.cpp
View File

@@ -60,8 +60,9 @@ int StepperDriver4PWM::init() {
}
// Set voltage to the pwm pin
void StepperDriver4PWM::setPwm(float Ualpha, float Ubeta) {
void StepperDriver4PWM::setPwm(float Ualpha, float Ubeta, float Uc) {
float duty_cycle1A(0.0f),duty_cycle1B(0.0f),duty_cycle2A(0.0f),duty_cycle2B(0.0f);
// limit the voltage in driver
Ualpha = _constrain(Ualpha, -voltage_limit, voltage_limit);
@@ -78,4 +79,4 @@ void StepperDriver4PWM::setPwm(float Ualpha, float Ubeta) {
duty_cycle2A = _constrain(abs(Ubeta)/voltage_power_supply,0.0f,1.0f);
// write to hardware
_writeDutyCycle4PWM(duty_cycle1A, duty_cycle1B, duty_cycle2A, duty_cycle2B, params);
}
}

3
firmware/lib/Arduino-FOC/src/drivers/StepperDriver4PWM.h
View File

@@ -45,7 +45,8 @@ class StepperDriver4PWM: public StepperDriver
* @param Ua phase A voltage
* @param Ub phase B voltage
*/
void setPwm(float Ua, float Ub) override;
void setPwm(float Ua, float Ub, float NOT_USED = NOT_SET) override;
// void setPwm(float Ua, float Ub) override;
private:

254
firmware/lib/currentsense/InlineCurrentSenseSync.cpp Normal file
View 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
View 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