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

start working on current sense

Browse Source
This commit is contained in:
matei jordache
2023-11-06 18:22:35 -05:00
parent d4948a7b3a
commit 1bec8bc5b7
15 changed files with 2878 additions and 3070 deletions
Download Patch File Download Diff File Expand all files Collapse all files

189
firmware/lib/currentsense/adc.c Normal file
View File

@@ -0,0 +1,189 @@
#include "adc.h"
ADC_HandleTypeDef hadc1;
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};
ADC_ChannelConfTypeDef sConfig = {0};
hadc1.Instance = ADC1;
hadc1.Init.ClockPrescaler = ADC_CLOCK_ASYNC_DIV16;
hadc1.Init.Resolution = ADC_RESOLUTION_12B;
hadc1.Init.DataAlign = ADC_DATAALIGN_RIGHT;
hadc1.Init.GainCompensation = 0;
hadc1.Init.ScanConvMode = ADC_SCAN_DISABLE;
hadc1.Init.EOCSelection = ADC_EOC_SINGLE_CONV;
hadc1.Init.LowPowerAutoWait = DISABLE;
hadc1.Init.ContinuousConvMode = DISABLE;
hadc1.Init.NbrOfConversion = 1;
hadc1.Init.DiscontinuousConvMode = DISABLE;
hadc1.Init.ExternalTrigConv = ADC_EXTERNALTRIG_T3_TRGO;
hadc1.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_RISING;
hadc1.Init.DMAContinuousRequests = ENABLE;
hadc1.Init.Overrun = ADC_OVR_DATA_PRESERVED;
hadc1.Init.OversamplingMode = DISABLE;
if (HAL_ADC_Init(&hadc1) != HAL_OK)
{
SIMPLEFOC_DEBUG("HAL ADC1 Init fail.");
}
multimode.Mode = ADC_MODE_INDEPENDENT;
if (HAL_ADCEx_MultiModeConfigChannel(&hadc1, &multimode) != HAL_OK)
{
SIMPLEFOC_DEBUG("HAL ADC1 Multimode configuration fail.");
}
sConfig.Channel = ADC_CHANNEL_VOPAMP1;
sConfig.Rank = ADC_REGULAR_RANK_1;
sConfig.SamplingTime = ADC_SAMPLETIME_2CYCLES_5;
sConfig.SingleDiff = ADC_SINGLE_ENDED;
sConfig.OffsetNumber = ADC_OFFSET_NONE;
sConfig.Offset = 0;
if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK)
{
Error_Handler();
}
}
void MX_ADC2_Init(void)
{
ADC_ChannelConfTypeDef sConfig = {0};
hadc2.Instance = ADC2;
hadc2.Init.ClockPrescaler = ADC_CLOCK_ASYNC_DIV16;
hadc2.Init.Resolution = ADC_RESOLUTION_12B;
hadc2.Init.DataAlign = ADC_DATAALIGN_RIGHT;
hadc2.Init.GainCompensation = 0;
hadc2.Init.ScanConvMode = ADC_SCAN_ENABLE;
hadc2.Init.EOCSelection = ADC_EOC_SINGLE_CONV;
hadc2.Init.LowPowerAutoWait = DISABLE;
hadc2.Init.ContinuousConvMode = DISABLE;
hadc2.Init.NbrOfConversion = 2;
hadc2.Init.DiscontinuousConvMode = DISABLE;
hadc2.Init.ExternalTrigConv = ADC_EXTERNALTRIG_T3_TRGO;
hadc2.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_RISING;
hadc2.Init.DMAContinuousRequests = ENABLE;
hadc2.Init.Overrun = ADC_OVR_DATA_PRESERVED;
hadc2.Init.OversamplingMode = DISABLE;
if (HAL_ADC_Init(&hadc2) != HAL_OK)
{
Error_Handler();
}
sConfig.Channel = ADC_CHANNEL_VOPAMP2;
sConfig.Rank = ADC_REGULAR_RANK_1;
sConfig.SamplingTime = ADC_SAMPLETIME_2CYCLES_5;
sConfig.SingleDiff = ADC_SINGLE_ENDED;
sConfig.OffsetNumber = ADC_OFFSET_NONE;
sConfig.Offset = 0;
if (HAL_ADC_ConfigChannel(&hadc2, &sConfig) != HAL_OK)
{
Error_Handler();
}
sConfig.Channel = ADC_CHANNEL_VOPAMP3_ADC2;
sConfig.Rank = ADC_REGULAR_RANK_2;
if (HAL_ADC_ConfigChannel(&hadc2, &sConfig) != HAL_OK)
{
Error_Handler();
}
}
void HAL_ADC_MspInit(ADC_HandleTypeDef* adcHandle)
{
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)
{
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);
}
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();
}
/* 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(adcHandle,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);
}
}

25
firmware/lib/currentsense/adc.h Normal file
View File

@@ -0,0 +1,25 @@
#ifndef __ADC_H__
#define __ADC_H__
#ifdef __cplusplus
extern "C" {
#endif
#include "stm32g4xx_hal.h"
#include "stm32g4xx_hal_adc.h"
extern ADC_HandleTypeDef hadc1;
extern ADC_HandleTypeDef hadc2;
extern DMA_HandleTypeDef hdma_adc1;
extern DMA_HandleTypeDef hdma_adc2;
void MX_ADC1_Init(void);
void MX_ADC2_Init(void);
#ifdef __cplusplus
}
#endif
#endif

26
firmware/lib/currentsense/dma.c Normal file
View File

@@ -0,0 +1,26 @@
#include "dma.h"
void MX_DMA_Init(void)
{
__HAL_RCC_DMAMUX1_CLK_ENABLE();
__HAL_RCC_DMA1_CLK_ENABLE();
HAL_NVIC_SetPriority(DMA1_Channel1_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(DMA1_Channel1_IRQn);
HAL_NVIC_SetPriority(DMA1_Channel2_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(DMA1_Channel2_IRQn);
}
extern "C" {
void DMA1_Channel1_IRQHandler(void)
{
HAL_DMA_IRQHandler(&hdma_adc1);
}
void DMA1_Channel2_IRQHandler(void)
{
HAL_DMA_IRQHandler(&hdma_adc2);
}
}

20
firmware/lib/currentsense/dma.h Normal file
View File

@@ -0,0 +1,20 @@
#ifndef __DMA_H__
#define __DMA_H__
#ifdef __cplusplus
extern "C" {
#endif
#include "stm32g4xx_hal.h"
#include "stm32g4xx_hal_dma.h"
void MX_DMA_Init(void);
void DMA1_Channel1_IRQHandler(void);
void DMA2_Channel2_IRQHandler(void);
#ifdef __cplusplus
}
#endif
#endif

77
firmware/lib/currentsense/opamp.c Normal file
View File

@@ -0,0 +1,77 @@
#include "opamp.h"
OPAMP_HandleTypeDef hopamp1;
OPAMP_HandleTypeDef hopamp2;
OPAMP_HandleTypeDef hopamp3;
void opamp_init(OPAMP_HandleTypeDef *hopamp, OPAMP_TypeDef *opamp)
{
hopamp1.Instance = opamp;
hopamp1.Init.PowerMode = OPAMP_POWERMODE_NORMALSPEED;
hopamp1.Init.Mode = OPAMP_PGA_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)
{
Error_Handler();
}
}
void configureOPAMPs(void)
{
opamp_init(&hopamp1, OPAMP1); // PA3
opamp_init(&hopamp2, OPAMP2); // PB0
opamp_init(&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);
}
}
void HAL_OPAMP_MspDeInit(OPAMP_HandleTypeDef* opampHandle)
{
if(opampHandle->Instance==OPAMP1)
{
HAL_GPIO_DeInit(GPIOA, GPIO_PIN_3);
}
else if(opampHandle->Instance==OPAMP2)
{
HAL_GPIO_DeInit(GPIOB, GPIO_PIN_0);
}
else if(opampHandle->Instance==OPAMP3)
{
HAL_GPIO_DeInit(GPIOA, GPIO_PIN_1);
}
}

22
firmware/lib/currentsense/opamp.h Normal file
View File

@@ -0,0 +1,22 @@
#ifndef __OPAMP_H__
#define __OPAMP_H__
#ifdef __cplusplus
extern "C" {
#endif
#include "stm32g4xx_hal.h"
#include "stm32g4xx_hal_opamp.h"
extern OPAMP_HandleTypeDef hopamp1;
extern OPAMP_HandleTypeDef hopamp2;
extern OPAMP_HandleTypeDef hopamp3;
void configureOPAMP(void);
#ifdef __cplusplus
}
#endif
#endif

79
firmware/lib/currentsense/utils.c Normal file
View File

@@ -0,0 +1,79 @@
#include "adc.h"
#include "opamp.h"
#include "dma.h"
#include "../stm32_mcu.h"
#include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h"
#include "communication/SimpleFOCDebug.h"
volatile uint16_t adc1Result = {0};
volatile uint16_t adc2Result[2] = {0};
float adcSens = 3.3f * 1.440f / 4096.0f;
float _readVoltageInline(const uint8_t pin, const void *cs_params)
{
switch (pin)
{
case PA3:
return adc1Result * adcSens;
break;
case PB0:
return adc2Result[0] * adcSens;
break;
case PA1:
return adc2Result[1] * adcSens;
break;
default:
return 0.0f;
break;
}
}
void *_configureADCInline(const void *driver_params, const int pinA, const int pinB, const int pinC)
{
_UNUSED(driver_params);
HAL_Init();
MX_GPIO_Init();
MX_DMA_Init();
MX_ADC1_Init(&hadc1);
MX_ADC2_Init(&hadc2);
configureOPAMPs();
MX_DMA1_Init(&hadc1, &hdma_adc1, DMA1_Channel1, DMA_REQUEST_ADC1);
MX_DMA1_Init(&hadc2, &hdma_adc2, DMA1_Channel2, DMA_REQUEST_ADC2);
if (HAL_ADC_Start_DMA(&hadc1, (uint32_t *)adc1Result, 1) != HAL_OK)
{
SIMPLEFOC_DEBUG("DMA read init failed");
}
if (HAL_ADC_Start_DMA(&hadc2, (uint32_t *)adc2Result, 2) != HAL_OK)
{
SIMPLEFOC_DEBUG("DMA read init failed");
}
HAL_OPAMP_Start(&hopamp1);
HAL_OPAMP_Start(&hopamp2);
HAL_OPAMP_Start(&hopamp3);
Stm32CurrentSenseParams *params = new Stm32CurrentSenseParams{
.pins = {pinA, pinB, pinC},
.adc_voltage_conv = (_ADC_VOLTAGE) / (_ADC_RESOLUTION),
.timer_handle = (HardwareTimer *)(HardwareTimer_Handle[get_timer_index(TIM3)]->__this)};
return params;
}
void *_configureADCInline(const void *driver_params, const int pinA, const int pinB, const int pinC)
{
_UNUSED(driver_params);
_UNUSED(pinA);
_UNUSED(pinB);
_UNUSED(pinC);
SIMPLEFOC_DEBUG("Lemon-Pepper does not use lowside sensing. Use inline current sense instead.");
return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED;
}

107
firmware/src/main.cpp
View File

@@ -36,14 +36,23 @@ extern uint8_t RxData[8];
// simpleFOC things
#define POLEPAIRS 50
#define RPHASE 3
#define MOTORKV 200
#define ENC_PPR 0xFFFD // 65533 -> 65534 ppr (65535 cause overflow on 16 bit timer)
#define MOTORKV 40
#define ENC_PPR 16383 // max 16384
SPIClass spi1(ENC_COPI, ENC_CIPO, ENC_SCK);
/**
* 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.)
*/
SPISettings myMT6835SPISettings(168000000/16, MT6835_BITORDER, SPI_MODE3);
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);
StepperDriver4PWM driver = StepperDriver4PWM(MOT_A1, MOT_A2, MOT_B1, MOT_B2);
StepperMotor motor = StepperMotor(POLEPAIRS, RPHASE, MOTORKV);
Commander commander = Commander(SerialUSB);
@@ -53,6 +62,7 @@ uint16_t counter = 0;
// Prototypes
void configureFOC(void);
void configureCAN(void);
void calibrateEncoder(void);
void setup()
{
@@ -68,11 +78,16 @@ void setup()
digitalWrite(MOT_EN, HIGH);
digitalWrite(CAL_EN, LOW);
// configureCAN();
// configureEncoder();
// configureFOC();
sensor.init(&spi1);
configureCAN();
configureFOC();
if(sensor.getABZResolution() != ENC_PPR){
digitalWrite(LED_FAULT, HIGH);
}
if(false){
calibrateEncoder();
}
// if(boardData.canID == 0x000)
// {
// // If the can ID is not initialized, then we'll look for a free ID.
@@ -89,13 +104,15 @@ void setup()
void loop()
{
// motor.loopFOC();
// motor.move();
// commander.run();
motor.loopFOC();
motor.move();
commander.run();
sensor.update();
delay(10);
SerialUSB.printf("%#06x\n", sensor.readRawAngle21());
if(counter == 0){
motor.target = -motor.target;
}
counter++;
#ifdef HAS_MONITOR
motor.monitor();
@@ -117,30 +134,34 @@ void configureFOC(void){
// Encoder initialization.
// Ideally configuring the sensor over SPI then use STM32HWEncoder
sensor.init(&spi1);
sensor.init();
sensor.setABZResolution(ENC_PPR);
enc.init();
// Driver initialization.
driver.pwm_frequency = 32000;
driver.voltage_power_supply = 9;
driver.voltage_power_supply = 12;
driver.voltage_limit = driver.voltage_power_supply/2;
driver.init();
// Motor PID parameters.
motor.PID_velocity.P = 0.2;
motor.PID_velocity.I = 3;
motor.PID_velocity.D = 0.002;
motor.PID_velocity.output_ramp = 100;
motor.LPF_velocity.Tf = 0.5;
motor.PID_velocity.P = 5;
motor.PID_velocity.I = 24;
motor.PID_velocity.D = 0.01;
motor.PID_velocity.output_ramp = 750;
motor.PID_velocity.limit = 10;
motor.LPF_velocity.Tf = 4;
motor.P_angle.P = 600;
motor.P_angle.limit = 10000;
motor.LPF_angle.Tf = 0; // try to avoid
// Motor initialization.
motor.voltage_sensor_align = 2;
motor.current_limit = 0.35;
motor.velocity_limit = 50;
motor.controller = MotionControlType::velocity_openloop;
// motor.voltage_sensor_align = 2;
motor.current_limit = 1;
motor.velocity_limit = 500;
motor.controller = MotionControlType::angle;
motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
// Monitor initialization
@@ -151,10 +172,15 @@ void configureFOC(void){
motor.monitor_downsample = 250;
#endif
motor.linkSensor(&enc);
motor.linkSensor(&sensor);
motor.linkDriver(&driver);
motor.target = 10;
// currentsense.linkDriver(&driver);
// currentsense.init();
// motor.linkCurrentSense(&currentsense);
motor.target = 3;
motor.zero_electric_angle = NOT_SET;
motor.sensor_direction = Direction::UNKNOWN;
@@ -183,4 +209,33 @@ void configureCAN(void){
FDCAN_Start(0x000);
}
void calibrateEncoder(void){
uint16_t calTime = micros();
motor.target = 35; // roughly 2000rpm -> need to write 0x1 to Reg. AUTOCAL_FREQ
MT6835Options4 currentSettings = sensor.getOptions4();
currentSettings.autocal_freq = 0x1;
sensor.setOptions4(currentSettings);
while (calTime - micros() < 2000000)
{
motor.loopFOC();
motor.move();
if(calTime - micros() > 2000){
// after motor is spinning at constant speed, enable calibration.
digitalWrite(CAL_EN, HIGH);
}
}
digitalWrite(CAL_EN, LOW);
uint8_t calibrationState = sensor.getCalibrationStatus();
if(calibrationState != 0x3){
digitalWrite(LED_FAULT, HIGH);
}
}