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;
// }

4
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()

8
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;

2
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

2
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);

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:

3
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);

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

55
firmware/src/main.cpp
View File

@@ -13,6 +13,7 @@
#include "can.h"
#include "dfu.h"
#include "utils.h"
#include "InlineCurrentSenseSync.h"
#include "lemon-pepper.h"
#define USBD_MANUFACTURER_STRING "matei repair lab"
@@ -44,6 +45,8 @@ extern uint8_t RxData[8];
#define MOTORKV 40
#define ENC_PPR 16383 // max 16383 (zero index) -> *4 for CPR, -1 is done in init to prevent rollover on 16 bit timer
#define SERIALPORT SerialUSB
/**
* SPI clockdiv of 16 gives ~10.5MHz clock. May still be stable with lower divisor.
* The HW encoder is configured using PPR, which is then *4 for CPR (full 12384 gives overflow on 16 bit timer.)
@@ -56,11 +59,11 @@ STM32HWEncoder enc = STM32HWEncoder(ENC_PPR, ENC_A, ENC_B, ENC_Z);
* The current sense amps have a gain of 90mA/V -> over 1.5A this is 135mA so we need gain of 24 to get full-scale.
* Actually we are limited to powers of 2 for gain. So it should be 16. This gives sensitivity of 1440mV/A.
* */
InlineCurrentSense currentsense = InlineCurrentSense(1440, ISENSE_U, ISENSE_V, ISENSE_W);
InlineCurrentSenseSync currentsense = InlineCurrentSenseSync(1440, ISENSE_U, ISENSE_V);
StepperDriver4PWM driver = StepperDriver4PWM(MOT_A1, MOT_A2, MOT_B1, MOT_B2);
StepperMotor motor = StepperMotor(POLEPAIRS);
Commander commander = Commander(SerialUSB);
Commander commander = Commander(SERIALPORT);
uint16_t counter = 0;
@@ -76,17 +79,23 @@ void setup()
pinMode(CAL_EN, OUTPUT);
pinMode(MOT_EN, OUTPUT);
SerialUSB.begin(115200);
SERIALPORT.begin(115200);
EEPROM.get(0, boardData);
digitalWrite(MOT_EN, HIGH);
digitalWrite(CAL_EN, LOW);
if (!configureCAN())
uint8_t ret = configureCAN();
if (!ret){
SIMPLEFOC_DEBUG("CAN init failed.");
if (!configureFOC())
digitalWrite(LED_FAULT, HIGH);
}
ret = configureFOC();
if (!ret){
SIMPLEFOC_DEBUG("FOC init failed.");
digitalWrite(LED_FAULT, HIGH);
}
if (sensor.getABZResolution() != ENC_PPR) // Check that PPR of the encoder matches our expectation.
{
@@ -137,16 +146,24 @@ void loop()
motor.move();
commander.run();
if (counter == 0xFFF)
{
digitalToggle(LED_GOOD);
SerialUSB.print(sensor.getAngle());
SerialUSB.print("\t");
SerialUSB.print(enc.getAngle());
SerialUSB.print("\t");
SerialUSB.println(sensor.getABZResolution());
// SerialUSB.printf("%d\t%d\t%d\n", sensor.getAngle(), sensor.getABZResolution(), enc.getAngle());
if (counter == 0xFF)
{
PhaseCurrent_s current = currentsense.getPhaseCurrents();
digitalToggle(LED_GOOD);
SERIALPORT.print(current.a);
SERIALPORT.print("\t");
SERIALPORT.print(current.b);
SERIALPORT.print("\t");
SERIALPORT.println(current.c);
// SERIALPORT.print(sensor.getAngle());
// SERIALPORT.print("\t");
// SERIALPORT.print(enc.getAngle());
// SERIALPORT.print("\t");
// SERIALPORT.println(sensor.getABZResolution());
// SERIALPORT.printf("%d\t%d\t%d\n", sensor.getAngle(), sensor.getABZResolution(), enc.getAngle());
counter = 0;
}
@@ -168,7 +185,7 @@ uint8_t configureFOC(void)
commander.verbose = VerboseMode::machine_readable;
#ifdef SIMPLEFOC_STM32_DEBUG
SimpleFOCDebug::enable(&SerialUSB);
SimpleFOCDebug::enable(&SERIALPORT);
#endif
// Encoder initialization.
@@ -227,7 +244,7 @@ uint8_t configureFOC(void)
// Monitor initialization
#ifdef HAS_MONITOR
motor.useMonitoring(SerialUSB);
motor.useMonitoring(SERIALPORT);
motor.monitor_start_char = 'M';
motor.monitor_end_char = 'M';
motor.monitor_downsample = 250;
@@ -283,13 +300,13 @@ uint8_t calibrateEncoder(void)
currentSettings.autocal_freq = 0x1;
sensor.setOptions4(currentSettings);
uint16_t calTime = micros();
while (calTime - micros() < 2000000)
uint32_t calTime = micros();
while ((micros() - calTime) < 2000000)
{
motor.loopFOC();
motor.move();
if (calTime - micros() > 2000)
if ((micros() -calTime) > 2000)
{
// after motor is spinning at constant speed, enable calibration.
digitalWrite(LED_GOOD, HIGH);