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

Merge pull request #9 from VIPQualityPost/currentsense

Browse Source 
Working current sense firmware
This commit is contained in:
VIPQualityPost
2023-11-17 20:47:25 -05:00
committed by GitHub
parent 4495b5a61d c825db4dd8
commit 780a601af4
24 changed files with 753 additions and 237 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) = 0;
// 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/current_sense/hardware_specific/generic_mcu.cpp
View File

@@ -1,4 +1,6 @@
#include "../hardware_api.h"
#include "communication/SimpleFOCDebug.h"
// function reading an ADC value and returning the read voltage
__attribute__((weak)) float _readADCVoltageInline(const int pinA, const void* cs_params){

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:

255
firmware/lib/currentsense/InlineCurrentSenseSync.cpp Normal file
View File

@@ -0,0 +1,255 @@
#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(){
SIMPLEFOC_DEBUG("CS:Init Current Sense Inline Sync");
// 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

146
firmware/lib/currentsense/adc.cpp
View File

@@ -5,8 +5,6 @@ ADC_HandleTypeDef hadc2;
DMA_HandleTypeDef hdma_adc1;
DMA_HandleTypeDef hdma_adc2;
uint32_t HAL_RCC_ADC12_CLK_ENABLED = 0;
void MX_ADC1_Init(void)
{
ADC_MultiModeTypeDef multimode = {0};
@@ -21,13 +19,17 @@ void MX_ADC1_Init(void)
hadc1.Init.EOCSelection = ADC_EOC_SINGLE_CONV;
hadc1.Init.LowPowerAutoWait = DISABLE;
hadc1.Init.ContinuousConvMode = DISABLE;
hadc1.Init.NbrOfConversion = 1;
hadc1.Init.NbrOfConversion = 3;
hadc1.Init.DiscontinuousConvMode = DISABLE;
hadc1.Init.ExternalTrigConv = ADC_EXTERNALTRIG_T3_TRGO;
hadc1.Init.ExternalTrigConv = ADC_EXTERNALTRIG_T2_TRGO;
hadc1.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_RISING;
hadc1.Init.DMAContinuousRequests = ENABLE;
hadc1.Init.Overrun = ADC_OVR_DATA_PRESERVED;
hadc1.Init.OversamplingMode = DISABLE;
hadc1.Init.Oversampling.Ratio = ADC_OVERSAMPLING_RATIO_16;
hadc1.Init.Oversampling.RightBitShift = ADC_RIGHTBITSHIFT_4;
hadc1.Init.Oversampling.TriggeredMode = ADC_TRIGGEREDMODE_SINGLE_TRIGGER;
hadc1.Init.Oversampling.OversamplingStopReset = ADC_REGOVERSAMPLING_CONTINUED_MODE;
if (HAL_ADC_Init(&hadc1) != HAL_OK)
SIMPLEFOC_DEBUG("HAL ADC1 Init fail.");
@@ -35,6 +37,7 @@ void MX_ADC1_Init(void)
if (HAL_ADCEx_MultiModeConfigChannel(&hadc1, &multimode) != HAL_OK)
SIMPLEFOC_DEBUG("HAL ADC1 multimode configuration fail.");
// sConfig.Channel = ADC_CHANNEL_VOPAMP1;
sConfig.Channel = ADC_CHANNEL_VOPAMP1;
sConfig.Rank = ADC_REGULAR_RANK_1;
sConfig.SamplingTime = ADC_SAMPLETIME_2CYCLES_5;
@@ -44,8 +47,13 @@ void MX_ADC1_Init(void)
if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK)
SIMPLEFOC_DEBUG("HAL ADC OPAMP1 init failed!");
sConfig.Channel = ADC_CHANNEL_TEMPSENSOR_ADC1;
sConfig.Channel = ADC_CHANNEL_3;
sConfig.Rank = ADC_REGULAR_RANK_2;
if(HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK)
SIMPLEFOC_DEBUG("HAL ADC Vmotor init failed!");
sConfig.Channel = ADC_CHANNEL_TEMPSENSOR_ADC1;
sConfig.Rank = ADC_REGULAR_RANK_3;
if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK)
SIMPLEFOC_DEBUG("HAL ADC temp init failed!");
}
@@ -65,11 +73,15 @@ void MX_ADC2_Init(void)
hadc2.Init.ContinuousConvMode = DISABLE;
hadc2.Init.NbrOfConversion = 2;
hadc2.Init.DiscontinuousConvMode = DISABLE;
hadc2.Init.ExternalTrigConv = ADC_EXTERNALTRIG_T3_TRGO;
hadc2.Init.ExternalTrigConv = ADC_EXTERNALTRIG_T2_TRGO;
hadc2.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_RISING;
hadc2.Init.DMAContinuousRequests = ENABLE;
hadc2.Init.Overrun = ADC_OVR_DATA_PRESERVED;
hadc2.Init.OversamplingMode = DISABLE;
hadc2.Init.Oversampling.Ratio = ADC_OVERSAMPLING_RATIO_16;
hadc2.Init.Oversampling.RightBitShift = ADC_RIGHTBITSHIFT_4;
hadc2.Init.Oversampling.TriggeredMode = ADC_TRIGGEREDMODE_SINGLE_TRIGGER;
hadc2.Init.Oversampling.OversamplingStopReset = ADC_REGOVERSAMPLING_CONTINUED_MODE;
if (HAL_ADC_Init(&hadc2) != HAL_OK)
SIMPLEFOC_DEBUG("HAL ADC2 init failed!");
@@ -88,95 +100,51 @@ void MX_ADC2_Init(void)
SIMPLEFOC_DEBUG("HAL ADC OPAMP3 init failed!");
}
void ADC_DMA_Init(ADC_HandleTypeDef* adcHandle)
uint32_t HAL_RCC_ADC12_CLK_ENABLED = 0;
void ADC_DMA_Init(void)
{
RCC_PeriphCLKInitTypeDef PeriphClkInit = {0};
if(adcHandle->Instance==ADC1)
PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_ADC12;
PeriphClkInit.Adc12ClockSelection = RCC_ADC12CLKSOURCE_SYSCLK;
if(HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit) != HAL_OK)
SIMPLEFOC_DEBUG("Error initializing peripheral clock in adc.cpp");
HAL_RCC_ADC12_CLK_ENABLED++;
if (HAL_RCC_ADC12_CLK_ENABLED == 1)
{
PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_ADC12;
PeriphClkInit.Adc12ClockSelection = RCC_ADC12CLKSOURCE_SYSCLK;
if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit) != HAL_OK)
{
Error_Handler();
}
HAL_RCC_ADC12_CLK_ENABLED++;
if(HAL_RCC_ADC12_CLK_ENABLED==1){
__HAL_RCC_ADC12_CLK_ENABLE();
}
/* ADC1 DMA Init */
/* ADC1 Init */
hdma_adc1.Instance = DMA1_Channel1;
hdma_adc1.Init.Request = DMA_REQUEST_ADC1;
hdma_adc1.Init.Direction = DMA_PERIPH_TO_MEMORY;
hdma_adc1.Init.PeriphInc = DMA_PINC_DISABLE;
hdma_adc1.Init.MemInc = DMA_MINC_ENABLE;
hdma_adc1.Init.PeriphDataAlignment = DMA_PDATAALIGN_HALFWORD;
hdma_adc1.Init.MemDataAlignment = DMA_MDATAALIGN_HALFWORD;
hdma_adc1.Init.Mode = DMA_NORMAL;
hdma_adc1.Init.Priority = DMA_PRIORITY_LOW;
if (HAL_DMA_Init(&hdma_adc1) != HAL_OK)
{
Error_Handler();
}
__HAL_LINKDMA(adcHandle,DMA_Handle,hdma_adc1);
__HAL_RCC_ADC12_CLK_ENABLE();
}
else if(adcHandle->Instance==ADC2)
{
PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_ADC12;
PeriphClkInit.Adc12ClockSelection = RCC_ADC12CLKSOURCE_SYSCLK;
if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit) != HAL_OK)
{
Error_Handler();
}
HAL_RCC_ADC12_CLK_ENABLED++;
if(HAL_RCC_ADC12_CLK_ENABLED==1){
__HAL_RCC_ADC12_CLK_ENABLE();
}
/* ADC1 DMA Init */
hdma_adc1.Instance = DMA1_Channel1;
hdma_adc1.Init.Request = DMA_REQUEST_ADC1;
hdma_adc1.Init.Direction = DMA_PERIPH_TO_MEMORY;
hdma_adc1.Init.PeriphInc = DMA_PINC_DISABLE;
hdma_adc1.Init.MemInc = DMA_MINC_ENABLE;
hdma_adc1.Init.PeriphDataAlignment = DMA_PDATAALIGN_HALFWORD;
hdma_adc1.Init.MemDataAlignment = DMA_MDATAALIGN_HALFWORD;
hdma_adc1.Init.Mode = DMA_CIRCULAR;
hdma_adc1.Init.Priority = DMA_PRIORITY_LOW;
if (HAL_DMA_Init(&hdma_adc1) != HAL_OK)
SIMPLEFOC_DEBUG("HAL error enabling DMA1 channel 1.");
/* ADC2 DMA Init */
/* ADC2 Init */
hdma_adc2.Instance = DMA1_Channel2;
hdma_adc2.Init.Request = DMA_REQUEST_ADC2;
hdma_adc2.Init.Direction = DMA_PERIPH_TO_MEMORY;
hdma_adc2.Init.PeriphInc = DMA_PINC_DISABLE;
hdma_adc2.Init.MemInc = DMA_MINC_ENABLE;
hdma_adc2.Init.PeriphDataAlignment = DMA_PDATAALIGN_HALFWORD;
hdma_adc2.Init.MemDataAlignment = DMA_MDATAALIGN_HALFWORD;
hdma_adc2.Init.Mode = DMA_CIRCULAR;
hdma_adc2.Init.Priority = DMA_PRIORITY_LOW;
if (HAL_DMA_Init(&hdma_adc2) != HAL_OK)
{
Error_Handler();
}
__HAL_LINKDMA(&hadc1, DMA_Handle, hdma_adc1);
__HAL_LINKDMA(adcHandle,DMA_Handle,hdma_adc2);
}
/* ADC2 DMA Init */
hdma_adc2.Instance = DMA1_Channel2;
hdma_adc2.Init.Request = DMA_REQUEST_ADC2;
hdma_adc2.Init.Direction = DMA_PERIPH_TO_MEMORY;
hdma_adc2.Init.PeriphInc = DMA_PINC_DISABLE;
hdma_adc2.Init.MemInc = DMA_MINC_ENABLE;
hdma_adc2.Init.PeriphDataAlignment = DMA_PDATAALIGN_HALFWORD;
hdma_adc2.Init.MemDataAlignment = DMA_MDATAALIGN_HALFWORD;
hdma_adc2.Init.Mode = DMA_CIRCULAR;
hdma_adc2.Init.Priority = DMA_PRIORITY_LOW;
if (HAL_DMA_Init(&hdma_adc2) != HAL_OK)
SIMPLEFOC_DEBUG("HAL error enabling DMA1 channel 2.");
__HAL_LINKDMA(&hadc2, DMA_Handle, hdma_adc2);
}
// void HAL_ADC_MspDeInit(ADC_HandleTypeDef* adcHandle)
// {
// if(adcHandle->Instance==ADC1)
// {
// HAL_RCC_ADC12_CLK_ENABLED--;
// if(HAL_RCC_ADC12_CLK_ENABLED==0){
// __HAL_RCC_ADC12_CLK_DISABLE();
// }
// HAL_DMA_DeInit(adcHandle->DMA_Handle);
// }
// else if(adcHandle->Instance==ADC2)
// {
// HAL_RCC_ADC12_CLK_ENABLED--;
// if(HAL_RCC_ADC12_CLK_ENABLED==0){
// __HAL_RCC_ADC12_CLK_DISABLE();
// }
// HAL_DMA_DeInit(adcHandle->DMA_Handle);
// }
// }

3
firmware/lib/currentsense/adc.h
View File

@@ -3,6 +3,7 @@
#include "stm32g4xx_hal.h"
#include "stm32g4xx_hal_adc.h"
#include "stm32g4xx_hal_adc_ex.h"
#include "communication/SimpleFOCDebug.h"
extern ADC_HandleTypeDef hadc1;
@@ -12,7 +13,7 @@ extern DMA_HandleTypeDef hdma_adc2;
void MX_ADC1_Init(void);
void MX_ADC2_Init(void);
void ADC_DMA_Init(ADC_HandleTypeDef* adcHandle);
void ADC_DMA_Init(void);
#endif

3
firmware/lib/currentsense/dma.cpp
View File

@@ -4,7 +4,7 @@ void MX_DMA_Init(void)
{
__HAL_RCC_DMAMUX1_CLK_ENABLE();
__HAL_RCC_DMA1_CLK_ENABLE();
__HAL_RCC_DMA2_CLK_ENABLE();
// __HAL_RCC_DMA2_CLK_ENABLE();
HAL_NVIC_SetPriority(DMA1_Channel1_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(DMA1_Channel1_IRQn);
@@ -16,7 +16,6 @@ void MX_DMA_Init(void)
PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_ADC12;
PeriphClkInit.Adc12ClockSelection = RCC_ADC12CLKSOURCE_SYSCLK;
HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit);
}
extern "C" {

93
firmware/lib/currentsense/opamp.cpp
View File

@@ -4,72 +4,73 @@ OPAMP_HandleTypeDef hopamp1;
OPAMP_HandleTypeDef hopamp2;
OPAMP_HandleTypeDef hopamp3;
void initOPAMP(OPAMP_HandleTypeDef *hopamp, OPAMP_TypeDef *opamp)
void configureOPAMPs(void)
{
hopamp1.Instance = opamp;
hopamp1.Instance = OPAMP1;
hopamp1.Init.PowerMode = OPAMP_POWERMODE_NORMALSPEED;
hopamp1.Init.Mode = OPAMP_PGA_MODE;
hopamp1.Init.Mode = OPAMP_FOLLOWER_MODE;
hopamp1.Init.NonInvertingInput = OPAMP_NONINVERTINGINPUT_IO1;
hopamp1.Init.InternalOutput = ENABLE;
hopamp1.Init.TimerControlledMuxmode = OPAMP_TIMERCONTROLLEDMUXMODE_DISABLE;
hopamp1.Init.PgaConnect = OPAMP_PGA_CONNECT_INVERTINGINPUT_NO;
hopamp1.Init.PgaGain = OPAMP_PGA_GAIN_16_OR_MINUS_15; // Adjust this to change the gains of the opamp.
hopamp1.Init.UserTrimming = OPAMP_TRIMMING_FACTORY;
if (HAL_OPAMP_Init(hopamp) != HAL_OK)
SIMPLEFOC_DEBUG("HAL OPAMP Init failed!");
if(HAL_OPAMP_Init(&hopamp1) != HAL_OK)
SIMPLEFOC_DEBUG("OPAMP1 init failed.");
hopamp2.Instance = OPAMP2;
hopamp2.Init.PowerMode = OPAMP_POWERMODE_NORMALSPEED;
hopamp2.Init.Mode = OPAMP_FOLLOWER_MODE;
hopamp2.Init.NonInvertingInput = OPAMP_NONINVERTINGINPUT_IO2;
hopamp2.Init.InternalOutput = ENABLE;
hopamp2.Init.TimerControlledMuxmode = OPAMP_TIMERCONTROLLEDMUXMODE_DISABLE;
hopamp2.Init.UserTrimming = OPAMP_TRIMMING_FACTORY;
if(HAL_OPAMP_Init(&hopamp2) != HAL_OK)
SIMPLEFOC_DEBUG("OPAMP2 init failed.");
hopamp3.Instance = OPAMP3;
hopamp3.Init.PowerMode = OPAMP_POWERMODE_NORMALSPEED;
hopamp3.Init.Mode = OPAMP_FOLLOWER_MODE;
hopamp3.Init.NonInvertingInput = OPAMP_NONINVERTINGINPUT_IO1;
hopamp3.Init.InternalOutput = ENABLE;
hopamp3.Init.TimerControlledMuxmode = OPAMP_TIMERCONTROLLEDMUXMODE_DISABLE;
hopamp3.Init.UserTrimming = OPAMP_TRIMMING_FACTORY;
if(HAL_OPAMP_Init(&hopamp3) != HAL_OK)
SIMPLEFOC_DEBUG("OPAMP3 init failed.");
}
void configureOPAMPs(void)
void OPAMP_GPIO_Init(void)
{
initOPAMP(&hopamp1, OPAMP1); // PA3
initOPAMP(&hopamp2, OPAMP2); // PB0
initOPAMP(&hopamp3, OPAMP3); // PA1
}
void HAL_OPAMP_MspInit(OPAMP_HandleTypeDef* opampHandle)
{
GPIO_InitTypeDef GPIO_InitStruct = {0};
if(opampHandle->Instance==OPAMP1)
{
__HAL_RCC_GPIOA_CLK_ENABLE();
GPIO_InitStruct.Pin = GPIO_PIN_3;
GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
}
else if(opampHandle->Instance==OPAMP2)
{
__HAL_RCC_GPIOB_CLK_ENABLE();
GPIO_InitStruct.Pin = GPIO_PIN_0;
GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
}
else if(opampHandle->Instance==OPAMP3)
{
__HAL_RCC_GPIOA_CLK_ENABLE();
GPIO_InitStruct.Pin = GPIO_PIN_1;
GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
}
__HAL_RCC_GPIOA_CLK_ENABLE();
GPIO_InitStruct.Pin = GPIO_PIN_2;
GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
GPIO_InitStruct.Pin = GPIO_PIN_3;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
__HAL_RCC_GPIOB_CLK_ENABLE();
GPIO_InitStruct.Pin = GPIO_PIN_0;
HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
GPIO_InitStruct.Pin = GPIO_PIN_13;
HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
}
void HAL_OPAMP_MspDeInit(OPAMP_HandleTypeDef* opampHandle)
void HAL_OPAMP_MspDeInit(OPAMP_HandleTypeDef *opampHandle)
{
if(opampHandle->Instance==OPAMP1)
if (opampHandle->Instance == OPAMP1)
{
HAL_GPIO_DeInit(GPIOA, GPIO_PIN_3);
}
else if(opampHandle->Instance==OPAMP2)
else if (opampHandle->Instance == OPAMP2)
{
HAL_GPIO_DeInit(GPIOB, GPIO_PIN_0);
}
else if(opampHandle->Instance==OPAMP3)
else if (opampHandle->Instance == OPAMP3)
{
HAL_GPIO_DeInit(GPIOA, GPIO_PIN_1);
HAL_GPIO_DeInit(GPIOB, GPIO_PIN_13);
}
}

1
firmware/lib/currentsense/opamp.h
View File

@@ -12,6 +12,7 @@ extern OPAMP_HandleTypeDef hopamp2;
extern OPAMP_HandleTypeDef hopamp3;
void configureOPAMPs(void);
void OPAMP_GPIO_Init(void);
#endif

60
firmware/lib/currentsense/utils.cpp
View File

@@ -7,10 +7,10 @@
#include "opamp.h"
float adcResolution = 4096.0f; // 12 bit ADC
float voltageScale = 3.3f; // full scale voltage range of ADC
float adcSens = adcResolution * voltageScale;
float voltageScale = 2.9f; // full scale voltage range of ADC
float adcSens = voltageScale / adcResolution;
volatile uint16_t adc1Result[2] = {0};
volatile uint16_t adc1Result[3] = {0};
volatile uint16_t adc2Result[2] = {0};
void MX_GPIO_Init(void)
@@ -20,30 +20,36 @@ void MX_GPIO_Init(void)
__HAL_RCC_GPIOA_CLK_ENABLE();
__HAL_RCC_GPIOB_CLK_ENABLE();
__HAL_RCC_GPIOC_CLK_ENABLE();
__HAL_RCC_ADC12_CLK_ENABLE();
OPAMP_GPIO_Init();
}
float _readVoltageInline(const uint8_t pin, const void *cs_params)
float _readADCVoltageInline(const int pin, const void *cs_params)
{
uint32_t rawResult;
// SIMPLEFOC_DEBUG("READ ADC VOLTAGE");
uint16_t rawResult;
switch (pin)
{
case PA3:
rawResult = adc1Result[0]; // ADC1 CH13 -> Vopamp1 internal output
rawResult = adc1Result[0];
break;
case PB13:
rawResult = adc2Result[0]; // ADC1 CH13 -> Vopamp1 internal output
break;
case PB0:
rawResult = adc2Result[0]; // ADC2 CH16 -> Vopamp2 internal output
rawResult = adc2Result[1]; // ADC2 CH16 -> Vopamp2 internal output
break;
case PA1:
rawResult = adc2Result[1]; // ADC2 CH18 -> Vopamp3 internal output
break;
// case PA13:
// rawResult = adc2Result[2]; // ADC2 CH18 -> Vopamp3 internal output
// break;
case PA2:
rawResult = adc1Result[1]; // ADC1 CH16 -> not sure what pin should represent this?
break;
// case PA2:
// rawResult = adc1Result[1]; // ADC1 CH16 -> not sure what pin should represent this?
// break;
default:
return 0.0f;
@@ -60,19 +66,25 @@ float _readVoltageLowSide(const int pinA, const void *cs_params)
void *_configureADCInline(const void *driver_params, const int pinA, const int pinB, const int pinC)
{
SIMPLEFOC_DEBUG("Configure Utils ADCInline");
_UNUSED(driver_params);
HAL_Init();
HAL_SYSCFG_VREFBUF_VoltageScalingConfig(SYSCFG_VREFBUF_VOLTAGE_SCALE2);
HAL_SYSCFG_EnableVREFBUF();
HAL_SYSCFG_VREFBUF_HighImpedanceConfig(SYSCFG_VREFBUF_HIGH_IMPEDANCE_DISABLE);
MX_GPIO_Init();
MX_DMA_Init();
MX_ADC1_Init();
MX_ADC2_Init();
configureOPAMPs();
ADC_DMA_Init(&hadc1);
ADC_DMA_Init(&hadc2);
ADC_DMA_Init();
if (HAL_ADC_Start_DMA(&hadc1, (uint32_t *)adc1Result, 1) != HAL_OK)
HAL_ADCEx_Calibration_Start(&hadc1, ADC_SINGLE_ENDED);
HAL_ADCEx_Calibration_Start(&hadc2, ADC_SINGLE_ENDED);
if (HAL_ADC_Start_DMA(&hadc1, (uint32_t *)adc1Result, 3) != HAL_OK)
{
SIMPLEFOC_DEBUG("DMA1 read init failed");
}
@@ -106,18 +118,28 @@ void *_configureADCLowSide(const void *driver_params, const int pinA, const int
void _driverSyncLowSide(void *_driver_params, void *_cs_params)
{
SIMPLEFOC_DEBUG("CS: Sync CS to driver");
STM32DriverParams* driver_params = (STM32DriverParams*)_driver_params;
Stm32CurrentSenseParams* cs_params = (Stm32CurrentSenseParams*)_cs_params;
_stopTimers(driver_params->timers, 6);
// See RM0440 pg. 1169
// This grabs the timer handle used for ADC and sets the direction bit as upcounting (?)
// Grab the timer handle used for ADC and sets the direction bit as upcounting
cs_params->timer_handle->getHandle()->Instance->CR1 |= TIM_CR1_DIR;
// This sets the value of the timer to the reload value. I think this is so that an event is immediately fired
/** Set the value of the timer to the reload value, so that overflow event is generated immediately.
* This is because we are using repetition counter to count every other event, skipping the underflow at
* valleys of PWM. We can downsample every other peak if repetition counter is increased to 3
*/
cs_params->timer_handle->getHandle()->Instance->CNT = cs_params->timer_handle->getHandle()->Instance->ARR;
// Set TIM_CR1_URS to 0b1 -> only update events are overflows/underflow
cs_params->timer_handle->getHandle()->Instance->CR1 |= 0x0004;
// Set TIM_CR2_MMS to 0b010 -> update event (overflow and underflow) mapped to tim2_trgo
cs_params->timer_handle->getHandle()->Instance->CR2 |= 0x0020;
_startTimers(driver_params->timers, 6);
}

9
firmware/lib/currentsense/utils.h
View File

@@ -6,9 +6,16 @@
#include "stm32g4xx_hal.h"
#include "communication/SimpleFOCDebug.h"
#include "current_sense/hardware_api.h"
// #include "current_sense/hardware_api.h"
#include "current_sense/hardware_specific/stm32/stm32_mcu.h"
#include "drivers/hardware_specific/stm32/stm32_mcu.h"
float _readADCVoltageInline(const int pinA, const void* cs_params);
// float _readADCVoltageInline(const uint8_t pin, const void *cs_params);
float _readVoltageLowSide(const int pinA, const void *cs_params);
void *_configureADCInline(const void *driver_params, const int pinA, const int pinB, const int pinC);
void *_configureADCLowSide(const void *driver_params, const int pinA, const int pinB, const int pinC);
void _driverSyncLowSide(void *_driver_params, void *_cs_params);
#endif
#endif

40
firmware/src/clock.c
View File

@@ -1,33 +1,33 @@
#include "pins_arduino.h"
/**
* @brief System Clock Configuration
* @retval None
*/
* @brief System Clock Configuration
* @retval None
*/
void SystemClock_Config(void)
{
RCC_OscInitTypeDef RCC_OscInitStruct = {0};
RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
#ifdef USBCON
RCC_PeriphCLKInitTypeDef PeriphClkInit = {};
#endif
/** Configure the main internal regulator output voltage
*/
*/
HAL_PWREx_ControlVoltageScaling(PWR_REGULATOR_VOLTAGE_SCALE1_BOOST);
/** Initializes the RCC Oscillators according to the specified parameters
* in the RCC_OscInitTypeDef structure.
*/
RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI|RCC_OSCILLATORTYPE_HSI48
|RCC_OSCILLATORTYPE_HSE;
* in the RCC_OscInitTypeDef structure.
*/
RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI48 | RCC_OSCILLATORTYPE_HSE;
RCC_OscInitStruct.HSEState = RCC_HSE_ON;
RCC_OscInitStruct.HSIState = RCC_HSI_ON;
RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
RCC_OscInitStruct.HSI48State = RCC_HSI48_ON;
RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
RCC_OscInitStruct.PLL.PLLM = RCC_PLLM_DIV1;
RCC_OscInitStruct.PLL.PLLN = 28;
RCC_OscInitStruct.PLL.PLLM = RCC_PLLM_DIV3;
RCC_OscInitStruct.PLL.PLLN = 85;
RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
RCC_OscInitStruct.PLL.PLLQ = RCC_PLLQ_DIV8;
RCC_OscInitStruct.PLL.PLLQ = RCC_PLLQ_DIV2;
RCC_OscInitStruct.PLL.PLLR = RCC_PLLR_DIV2;
if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
{
@@ -35,9 +35,8 @@ void SystemClock_Config(void)
}
/** Initializes the CPU, AHB and APB buses clocks
*/
RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
|RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
*/
RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2;
RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV1;
@@ -47,4 +46,13 @@ void SystemClock_Config(void)
{
Error_Handler();
}
#ifdef USBCON
/* Initializes the peripherals clocks */
PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_USB;
PeriphClkInit.UsbClockSelection = RCC_USBCLKSOURCE_HSI48;
if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit) != HAL_OK) {
Error_Handler();
}
#endif
}

106
firmware/src/main.cpp
View File

@@ -3,8 +3,8 @@
#include <SPI.h>
#include <SimpleFOC.h>
#include <SimpleFOCDrivers.h>
#include "encoders/MT6835/MagneticSensorMT6835.h"
#include "SimpleFOCDrivers.h"
#include "encoders/mt6835/MagneticSensorMT6835.h"
#include "encoders/stm32hwencoder/STM32HWEncoder.h"
#include "stm32g4xx_hal_conf.h"
@@ -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"
@@ -35,8 +36,8 @@ uint8_t updateData = 0;
const uint16_t magicWord = 0xAF0C;
// canbus things
extern uint8_t TxData[8];
extern uint8_t RxData[8];
extern volatile uint8_t TxData[8];
extern volatile uint8_t RxData[8];
// simpleFOC things
#define POLEPAIRS 50
@@ -44,6 +45,10 @@ 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 Serial3
HardwareSerial Serial3 = HardwareSerial(PB8, PB9);
/**
* 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.)
@@ -53,40 +58,58 @@ MagneticSensorMT6835 sensor = MagneticSensorMT6835(ENC_CS, myMT6835SPISettings);
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);
* TODO: Change the current sense code to reflect the new inline current sense amplifier choice with sense resistor.
*/
// InlineCurrentSenseSync currentsense = InlineCurrentSenseSync(90, ISENSE_U, ISENSE_V, ISENSE_W);
StepperDriver4PWM driver = StepperDriver4PWM(MOT_A1, MOT_A2, MOT_B1, MOT_B2);
// StepperMotor motor = StepperMotor(POLEPAIRS, RPHASE, MOTORKV, 0.0045);
StepperMotor motor = StepperMotor(POLEPAIRS);
Commander commander = Commander(SerialUSB);
Commander commander = Commander(SERIALPORT);
uint16_t counter = 0;
extern volatile uint16_t adc1Result[3];
extern volatile uint16_t adc2Result[2];
DQCurrent_s foc_currents;
float electrical_angle;
PhaseCurrent_s phase_currents;
// Prototypes
uint8_t configureFOC(void);
uint8_t configureCAN(void);
uint8_t calibrateEncoder(void);
void userButton(void);
void setup()
{
pinMode(LED_GOOD, OUTPUT);
pinMode(LED_FAULT, OUTPUT);
pinMode(CAL_EN, OUTPUT);
pinMode(MOT_EN, OUTPUT);
pinMode(USER_BUTTON, INPUT);
SerialUSB.begin(115200);
attachInterrupt(USER_BUTTON, userButton, RISING);
SERIALPORT.begin(115200);
EEPROM.get(0, boardData);
digitalWrite(MOT_EN, HIGH);
digitalWrite(CAL_EN, LOW);
if (!configureCAN())
SIMPLEFOC_DEBUG("CAN init failed.");
if (!configureFOC())
uint8_t ret;
// ret = configureCAN();
// if (!ret){
// SIMPLEFOC_DEBUG("CAN init failed.");
// 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,21 +160,16 @@ 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());
electrical_angle = motor.electricalAngle();
// phase_currents = currentsense.getPhaseCurrents();
// foc_currents = currentsense.getFOCCurrents(electrical_angle);
// SerialUSB.printf("%d\t%d\t%d\n", sensor.getAngle(), sensor.getABZResolution(), enc.getAngle());
if(counter == 0xFFF){
digitalToggle(LED_GOOD);
// Serial.println(adc1Result[0]);
counter = 0;
}
counter++;
counter++;
#ifdef HAS_MONITOR
motor.monitor();
#endif
@@ -168,7 +186,7 @@ uint8_t configureFOC(void)
commander.verbose = VerboseMode::machine_readable;
#ifdef SIMPLEFOC_STM32_DEBUG
SimpleFOCDebug::enable(&SerialUSB);
SimpleFOCDebug::enable(&SERIALPORT);
#endif
// Encoder initialization.
@@ -207,15 +225,17 @@ uint8_t configureFOC(void)
driver.init();
// Motor PID parameters.
motor.PID_velocity.P = 5;
motor.PID_velocity.I = 24;
motor.PID_velocity.D = 0.01;
motor.PID_velocity.P = 0.035;
motor.PID_velocity.I = 0.01;
motor.PID_velocity.D = 0.000;
motor.LPF_velocity.Tf = 0.004;
motor.PID_velocity.output_ramp = 750;
motor.PID_velocity.limit = 500;
motor.LPF_velocity.Tf = 4;
motor.P_angle.P = 600;
motor.P_angle.limit = 10000;
motor.P_angle.P = 350;
motor.P_angle.I = 8;
motor.P_angle.D = 0.3;
motor.LPF_angle = 0.001;
motor.LPF_angle.Tf = 0; // try to avoid
// Motor initialization.
@@ -227,7 +247,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;
@@ -236,10 +256,10 @@ uint8_t configureFOC(void)
motor.linkSensor(&sensor);
motor.linkDriver(&driver);
currentsense.linkDriver(&driver);
currentsense.init();
motor.linkCurrentSense(&currentsense);
// currentsense.linkDriver(&driver);
// int ret = currentsense.init();
// SERIALPORT.printf("Current Sense init result: %i\n", ret);
// motor.linkCurrentSense(&currentsense);
motor.target = 10;
@@ -283,13 +303,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);
@@ -302,3 +322,9 @@ uint8_t calibrateEncoder(void)
return sensor.getCalibrationStatus();
}
void userButton(void)
{
if(USB->DADDR != 0)
jump_to_bootloader();
}

34
hardware/currentsense.kicad_sch
View File

@@ -648,6 +648,10 @@
(stroke (width 0) (type default))
(uuid 729cc6be-8aa4-41f4-be98-ff91b8dfb517)
)
(wire (pts (xy 148.59 127) (xy 157.48 127))
(stroke (width 0) (type default))
(uuid 72c65ad4-7171-4f7e-9fdb-92b5a6e36ed1)
)
(wire (pts (xy 208.28 135.89) (xy 208.28 139.7))
(stroke (width 0) (type default))
(uuid 76cc1c25-d03c-4186-9987-57e46edfc330)
@@ -792,6 +796,10 @@
(stroke (width 0) (type default))
(uuid fef7ec77-0cf9-49f6-9253-634118909a52)
)
(wire (pts (xy 148.59 91.44) (xy 157.48 91.44))
(stroke (width 0) (type default))
(uuid ff03524d-69d1-4f45-9362-ad5afe606fd8)
)
(text "join at power connector" (at 189.23 127 0)
(effects (font (size 1.27 1.27)) (justify left bottom))
@@ -841,6 +849,32 @@
(uuid f27d6576-8dc6-4b35-a6ee-606d878ff90e)
)
(symbol (lib_id "Device:R_Small_US") (at 146.05 127 90) (unit 1)
(in_bom yes) (on_board yes) (dnp no) (fields_autoplaced)
(uuid 084a4821-5263-43df-98a8-7fb0f03d0a22)
(property "Reference" "R703" (at 146.05 120.65 90)
(effects (font (size 1.27 1.27)))
)
(property "Value" "20R" (at 146.05 123.19 90)
(effects (font (size 1.27 1.27)))
)
(property "Footprint" "Resistor_SMD:R_0402_1005Metric" (at 146.05 127 0)
(effects (font (size 1.27 1.27)) hide)
)
(property "Datasheet" "~" (at 146.05 127 0)
(effects (font (size 1.27 1.27)) hide)
)
(pin "1" (uuid d71cd166-4c52-45d6-aaa3-2fa89ff75908))
(pin "2" (uuid cd22acb1-7602-45f3-97fa-03db382a1c81))
(instances
(project "lemon-pepper"
(path "/0306e2fa-4433-4288-91d9-65a3484207ad/41d11f3b-333a-47c0-a126-1f991ee11e83"
(reference "R703") (unit 1)
)
)
)
)
(symbol (lib_id "power:PWR_FLAG") (at 193.04 135.89 90) (unit 1)
(in_bom yes) (on_board yes) (dnp no) (fields_autoplaced)
(uuid 08d81450-96ad-4d2c-9c7a-dc9ecb10ad44)

110
hardware/lemon-pepper.kicad_pcb
View File

@@ -3948,6 +3948,60 @@
)
)
(footprint "Resistor_SMD:R_0402_1005Metric" (layer "F.Cu")
(tstamp 95bca4c9-8b19-49ca-886b-ad8247c70844)
(at 151.75 80.575 180)
(descr "Resistor SMD 0402 (1005 Metric), square (rectangular) end terminal, IPC_7351 nominal, (Body size source: IPC-SM-782 page 72, https://www.pcb-3d.com/wordpress/wp-content/uploads/ipc-sm-782a_amendment_1_and_2.pdf), generated with kicad-footprint-generator")
(tags "resistor")
(property "Sheetfile" "currentsense.kicad_sch")
(property "Sheetname" "current sense")
(property "ki_description" "Resistor, small US symbol")
(property "ki_keywords" "r resistor")
(path "/41d11f3b-333a-47c0-a126-1f991ee11e83/7ea0b72c-9d2c-4584-ae0c-f1c3223e17c3")
(attr smd)
(fp_text reference "R702" (at 0 -1.17) (layer "F.SilkS") hide
(effects (font (size 1 1) (thickness 0.15)))
(tstamp 6247f1a6-0294-4105-931c-92275fb1acbf)
)
(fp_text value "5k" (at 0 1.17) (layer "F.Fab")
(effects (font (size 1 1) (thickness 0.15)))
(tstamp 642f1893-5ce5-413d-9338-36db9b299530)
)
(fp_text user "${REFERENCE}" (at 0 0) (layer "F.Fab")
(effects (font (size 0.26 0.26) (thickness 0.04)))
(tstamp f04b372a-aa46-4335-b283-077355704754)
)
(fp_line (start -0.153641 -0.38) (end 0.153641 -0.38)
(stroke (width 0.12) (type solid)) (layer "F.SilkS") (tstamp d44eb6de-0228-4a23-8700-80099de6f412))
(fp_line (start -0.153641 0.38) (end 0.153641 0.38)
(stroke (width 0.12) (type solid)) (layer "F.SilkS") (tstamp cd65f080-b41e-42da-841b-46c1e4551fa5))
(fp_line (start -0.93 -0.47) (end 0.93 -0.47)
(stroke (width 0.05) (type solid)) (layer "F.CrtYd") (tstamp 62d252bc-0199-400c-962b-fd91f823379b))
(fp_line (start -0.93 0.47) (end -0.93 -0.47)
(stroke (width 0.05) (type solid)) (layer "F.CrtYd") (tstamp 07b7d8eb-2037-47dd-9e90-c40d4ee11a72))
(fp_line (start 0.93 -0.47) (end 0.93 0.47)
(stroke (width 0.05) (type solid)) (layer "F.CrtYd") (tstamp 46ab5a3a-88d6-4848-b791-26903365f95d))
(fp_line (start 0.93 0.47) (end -0.93 0.47)
(stroke (width 0.05) (type solid)) (layer "F.CrtYd") (tstamp 51ec5b0d-5582-448c-96ef-8cb6e953e93f))
(fp_line (start -0.525 -0.27) (end 0.525 -0.27)
(stroke (width 0.1) (type solid)) (layer "F.Fab") (tstamp 5e28671b-da8e-4f5d-835c-b69ae7c8cfc9))
(fp_line (start -0.525 0.27) (end -0.525 -0.27)
(stroke (width 0.1) (type solid)) (layer "F.Fab") (tstamp cdc0b3d3-45f5-406e-845e-3a19569b81d8))
(fp_line (start 0.525 -0.27) (end 0.525 0.27)
(stroke (width 0.1) (type solid)) (layer "F.Fab") (tstamp b4aade76-17cd-4f85-b3f9-af5de67d2c96))
(fp_line (start 0.525 0.27) (end -0.525 0.27)
(stroke (width 0.1) (type solid)) (layer "F.Fab") (tstamp 8623440b-838d-4c09-a92e-da5c344f18e0))
(pad "1" smd roundrect (at -0.51 0 180) (size 0.54 0.64) (layers "F.Cu" "F.Paste" "F.Mask") (roundrect_rratio 0.25)
(net 89 "Net-(U702-VIOUT)") (pintype "passive") (tstamp c18b3391-9d17-444a-a37c-f9b574b9b512))
(pad "2" smd roundrect (at 0.51 0 180) (size 0.54 0.64) (layers "F.Cu" "F.Paste" "F.Mask") (roundrect_rratio 0.25)
(net 17 "/current sense/V_SENSE") (pintype "passive") (tstamp 2499fba1-b026-4fce-b16c-c335e0e017c2))
(model "${KICAD6_3DMODEL_DIR}/Resistor_SMD.3dshapes/R_0402_1005Metric.wrl"
(offset (xyz 0 0 0))
(scale (xyz 1 1 1))
(rotate (xyz 0 0 0))
)
)
(footprint "mechanical:NEMA14" locked (layer "F.Cu")
(tstamp 95c5292d-b574-4ab1-ba30-310faa18e02f)
(at 146.3 86.975)
@@ -6024,6 +6078,60 @@
)
)
(footprint "Resistor_SMD:R_0402_1005Metric" (layer "F.Cu")
(tstamp e69ae357-baf4-4ca2-a29c-a3d90a58930b)
(at 141.625 80.575 180)
(descr "Resistor SMD 0402 (1005 Metric), square (rectangular) end terminal, IPC_7351 nominal, (Body size source: IPC-SM-782 page 72, https://www.pcb-3d.com/wordpress/wp-content/uploads/ipc-sm-782a_amendment_1_and_2.pdf), generated with kicad-footprint-generator")
(tags "resistor")
(property "Sheetfile" "currentsense.kicad_sch")
(property "Sheetname" "current sense")
(property "ki_description" "Resistor, small US symbol")
(property "ki_keywords" "r resistor")
(path "/41d11f3b-333a-47c0-a126-1f991ee11e83/54a1fb90-bae5-4dd5-8295-b6d0f7fa3ae2")
(attr smd)
(fp_text reference "R701" (at 0 -1.17) (layer "F.SilkS") hide
(effects (font (size 1 1) (thickness 0.15)))
(tstamp 84555ffc-f97f-4e2f-8445-c17efa744be1)
)
(fp_text value "5k" (at 0 1.17) (layer "F.Fab")
(effects (font (size 1 1) (thickness 0.15)))
(tstamp a9d736e7-0902-4bf8-88ca-1e3cd6add12d)
)
(fp_text user "${REFERENCE}" (at 0 0) (layer "F.Fab")
(effects (font (size 0.26 0.26) (thickness 0.04)))
(tstamp 15d2c22e-0045-4429-9b25-506eb887bde5)
)
(fp_line (start -0.153641 -0.38) (end 0.153641 -0.38)
(stroke (width 0.12) (type solid)) (layer "F.SilkS") (tstamp 5a66ac82-6db9-43ca-b684-c72e5afc7def))
(fp_line (start -0.153641 0.38) (end 0.153641 0.38)
(stroke (width 0.12) (type solid)) (layer "F.SilkS") (tstamp acfb1a97-46ed-4da5-8d73-c3ecafb9650c))
(fp_line (start -0.93 -0.47) (end 0.93 -0.47)
(stroke (width 0.05) (type solid)) (layer "F.CrtYd") (tstamp 7ce96ed2-9552-4191-89d8-2e72893cbd45))
(fp_line (start -0.93 0.47) (end -0.93 -0.47)
(stroke (width 0.05) (type solid)) (layer "F.CrtYd") (tstamp 7fa0c713-2d68-4d7d-8c76-f3f09fcd26ef))
(fp_line (start 0.93 -0.47) (end 0.93 0.47)
(stroke (width 0.05) (type solid)) (layer "F.CrtYd") (tstamp de148a6d-f502-41c2-aa7c-a7abefca156b))
(fp_line (start 0.93 0.47) (end -0.93 0.47)
(stroke (width 0.05) (type solid)) (layer "F.CrtYd") (tstamp 8ab22f38-253d-42a6-a5e7-a298bc69cd2a))
(fp_line (start -0.525 -0.27) (end 0.525 -0.27)
(stroke (width 0.1) (type solid)) (layer "F.Fab") (tstamp bf5fe7db-4045-4d45-bfb3-8e8251d79bdc))
(fp_line (start -0.525 0.27) (end -0.525 -0.27)
(stroke (width 0.1) (type solid)) (layer "F.Fab") (tstamp 5f3f064d-940f-4e87-b3cc-ad14fae28ed3))
(fp_line (start 0.525 -0.27) (end 0.525 0.27)
(stroke (width 0.1) (type solid)) (layer "F.Fab") (tstamp 092d6890-db08-4c9f-9651-658bb81d65ba))
(fp_line (start 0.525 0.27) (end -0.525 0.27)
(stroke (width 0.1) (type solid)) (layer "F.Fab") (tstamp 50bbe285-0061-4deb-880b-6572d06e334a))
(pad "1" smd roundrect (at -0.51 0 180) (size 0.54 0.64) (layers "F.Cu" "F.Paste" "F.Mask") (roundrect_rratio 0.25)
(net 88 "Net-(U701-VIOUT)") (pintype "passive") (tstamp 4aa895f7-e80a-4a4e-b224-5d13b7e92f23))
(pad "2" smd roundrect (at 0.51 0 180) (size 0.54 0.64) (layers "F.Cu" "F.Paste" "F.Mask") (roundrect_rratio 0.25)
(net 16 "/current sense/U_SENSE") (pintype "passive") (tstamp fd1d151d-85f9-42b9-b634-e3adabe80824))
(model "${KICAD6_3DMODEL_DIR}/Resistor_SMD.3dshapes/R_0402_1005Metric.wrl"
(offset (xyz 0 0 0))
(scale (xyz 1 1 1))
(rotate (xyz 0 0 0))
)
)
(footprint "Resistor_SMD:R_0402_1005Metric" (layer "F.Cu")
(tstamp eff89581-ca64-4897-b991-183a6047b0b7)
(at 149.625 91.75)
@@ -7418,11 +7526,13 @@
(segment (start 155.91038 94.385) (end 156.395 94.385) (width 0.25) (layer "F.Cu") (net 1) (tstamp e2bd038f-6de5-487d-950e-026bc6461a01))
(segment (start 136.575 76.395) (end 136.61 76.395) (width 0.25) (layer "F.Cu") (net 1) (tstamp e43385e3-da36-4f95-be0c-a8a3ae4b8711))
(segment (start 141.25 103.75) (end 140.44283 102.94283) (width 0.25) (layer "F.Cu") (net 1) (tstamp e4454056-1b5c-4fe8-8d26-a1180a03800a))
(segment (start 146.625 83.64038) (end 147.095 83.17038) (width 0.25) (layer "F.Cu") (net 1) (tstamp e4719c62-c6da-41ca-9408-7e6930f33b77))
(segment (start 152.472649 82.344096) (end 153.3 83.171447) (width 0.25) (layer "F.Cu") (net 1) (tstamp e4a9dff8-4710-4c5e-8b26-a683cf6f4897))
(segment (start 155.425 84.225) (end 154 84.225) (width 0.25) (layer "F.Cu") (net 1) (tstamp e64208e8-a8cb-4e94-a5b2-dbf8b34295c6))
(segment (start 147.775 93.1) (end 149.31 93.1) (width 0.25) (layer "F.Cu") (net 1) (tstamp e70c6643-48cc-4c3a-9c9c-8aaa2ae8253b))
(segment (start 142.0605 86.925) (end 142.05 86.9145) (width 0.25) (layer "F.Cu") (net 1) (tstamp e75bdc4e-838b-402f-905a-e2f2c94b3d38))
(segment (start 154.59 97.295) (end 154.59 97.39) (width 0.25) (layer "F.Cu") (net 1) (tstamp e8dbc1e0-802e-494b-8b9b-105e03bf313d))
(segment (start 146.625 84.2) (end 146.625 83.64038) (width 0.25) (layer "F.Cu") (net 1) (tstamp e93d71b6-80da-43c4-8d73-ca538df56daf))
(segment (start 138.76 91.84) (end 138.76 91.04) (width 0.25) (layer "F.Cu") (net 1) (tstamp e9d2e006-b8c3-44df-827c-6e0e3dedaa32))
(segment (start 147.275 83.43) (end 147.275 84.2) (width 0.25) (layer "F.Cu") (net 1) (tstamp ea4475b3-0d49-4cfa-82fc-cfa5a51cd07a))
(segment (start 136.388533 93.008293) (end 136.883507 93.008293) (width 0.25) (layer "F.Cu") (net 1) (tstamp eb4cc686-22a8-4de1-af9f-411e18b7abe3))

2
hardware/lemon-pepper.kicad_prl
View File

@@ -1,6 +1,6 @@
{
"board": {
"active_layer": 2,
"active_layer": 0,
"active_layer_preset": "",
"auto_track_width": true,
"hidden_netclasses": [],

5
platformio.ini
View File

@@ -10,7 +10,6 @@
[platformio]
default_envs = lemon-pepper
boards_dir = firmware/boards
src_dir = firmware/src
lib_dir = firmware/lib
include_dir = firmware/include
@@ -19,7 +18,7 @@ test_dir = firmware/test
[env:lemon-pepper]
platform = ststm32
board = genericSTM32G431CB
board_build.f_cpu = 168000000
; board_build.f_cpu = 170000000
framework = arduino
upload_protocol = stlink
debug_tool = stlink
@@ -34,10 +33,12 @@ build_flags =
-D PIO_FRAMEWORK_ARDUINO_ENABLE_CDC
-D HAL_FDCAN_MODULE_ENABLED
-D HAL_OPAMP_MODULE_ENABLED
-D HSE_VALUE=12000000U
-D FDCAN_ALT1
-D SN65HVD23x
-D ARDUINO_GENERIC_G431CBUX
-D SIMPLEFOC_STM32_CUSTOMCURRENTSENSE
-D LEMONPEPPER
; -D PCB_REV1 ; or PCB_REV2
; -D HAS_MONITOR