Hello,
I am having issues with controlling the stepper motor in closed-loop mode, as the motor runs very unevenly there and only allows speeds up to 20 rad/s. It performs worse than when I run the motor in open-loop mode.
A brief overview of the hardware: I have designed a PCB myself, which features an STM32G431CBT6 microcontroller in combination with two DRV8874PWPR H-bridges (integrated low-side current sensing) and an AS5047P magnetic sensor.
Unfortunately, I only have the USBD+/- connections for data, and I cannot configure a virtual COM port (which unfortunately doesn’t work for me in the Arduino IDE), meaning I don’t have debug output. However, I am fortunate to have access to I2C, so I can send some information to a Raspberry Pi Pico and also output data. Because of this, I know that the initialization methods are not reporting errors, and I was able to verify the sensor output. I couldn’t find any issues there.
What I found strange is that when I specify the winding resistance, the motor runs even worse. I am sending you the code I am using—does anyone have any ideas on what could be causing this, or has anyone encountered a similar problem?
I would appreciate any help.
Here the code:
#include “stm32g4xx_hal.h”
#include “stm32g4xx_hal_rcc.h”
#include “usbd_cdc_if.h”
#include “usbd_cdc.h”
#include <SimpleFOC.h>
#include <SPI.h>
#include <Wire.h>
#undef HSE_VALUE
#define HSE_VALUE ((uint32_t)8000000) // set 8 MHz external ceramic resonator frequency
#define PHASE_A_PIN1 PA3
#define PHASE_A_PIN2 PA2
#define PHASE_B_PIN1 PA1
#define PHASE_B_PIN2 PA0
#define ENABLE_A_PIN PA4
#define ENABLE_B_PIN PA6
#define AS5047_CS PA15 // SPI chip select
#define AS5047_MOSI PB5 // SPI MOSI
#define AS5047_MISO PB4 // SPI MISO
#define AS5047_SCK PB3 // SPI SCK
#define I2C_ADDRESS 0x42
PCD_HandleTypeDef hpcd_USB_FS;
I2C_HandleTypeDef hi2c2;
SPIClass SPI3Instance(AS5047_MOSI, AS5047_MISO, AS5047_SCK);
StepperMotor motor = StepperMotor(50); // 50 = number of pole pairs
StepperDriver4PWM driver = StepperDriver4PWM(PHASE_A_PIN1, PHASE_A_PIN2, PHASE_B_PIN1, PHASE_B_PIN2, ENABLE_A_PIN, ENABLE_B_PIN);
MagneticSensorSPI sensor = MagneticSensorSPI(AS5047_CS, 14, 0X3FFF);
LowsideCurrentSense current_sense = LowsideCurrentSense(4.44444, PB0, PB1);
void SystemClock_Config(void);
static void MX_GPIO_Init(void);
static void MX_USB_PCD_Init(void);
static void MX_I2C2_Init(void);
void setup() {
HAL_Init();
SystemClock_Config();
MX_GPIO_Init();
MX_USB_PCD_Init();
Wire.setSCL(PA9);
Wire.setSDA(PA8);
Wire.begin();
pinMode(PA7, INPUT); // set H-bridge 1 mode to independent half bridge (HIGH-Z state)
pinMode(PA5, INPUT); // set H-bridge 2 mode to independent half bridge (HIGH-Z state)
pinMode(PB11, OUTPUT);
driver.voltage_power_supply = 12;
driver.init();
sensor.spi_mode = SPI_MODE1;
sensor.init(&SPI3Instance);
current_sense.linkDriver(&driver);
motor.linkSensor(&sensor);
motor.linkDriver(&driver);
motor.voltage_limit = 10;
motor.controller = MotionControlType::velocity;// MotionControlType::velocity_openloop;
motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
if(!motor.init()){
digitalWrite(PB11, HIGH);
}
if(!current_sense.init()){
digitalWrite(PB11, HIGH);
}
motor.linkCurrentSense(¤t_sense);
//motor.PID_velocity.P = 0.2f;
//motor.PID_velocity.I = 20;
//motor.PID_velocity.D = 0;
motor.LPF_velocity.Tf = 0.01f;
//motor.P_angle.P = 20;
// motor.velocity_limit = 20;
// motor.voltage_sensor_align = 3;
if(!motor.initFOC()){
digitalWrite(PB11, HIGH);
}
}
void loop() {
motor.loopFOC();
motor.move(20);
/*
float angle = sensor.getAngle();
uint8_t data[sizeof(angle)];
memcpy(data, &angle, sizeof(angle));
Wire.beginTransmission(I2C_ADDRESS);
Wire.write(data, sizeof(data));
Wire.endTransmission();
delay(100);
*/
}
void SystemClock_Config(void) {
RCC_OscInitTypeDef RCC_OscInitStruct = { 0 };
RCC_ClkInitTypeDef RCC_ClkInitStruct = { 0 };
/** Configure the main internal regulator output voltage */
HAL_PWREx_ControlVoltageScaling(PWR_REGULATOR_VOLTAGE_SCALE1);
/** Initializes the RCC Oscillators according to the specified parameters
- in the RCC_OscInitTypeDef structure.
*/
RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI | RCC_OSCILLATORTYPE_HSE;
RCC_OscInitStruct.HSEState = RCC_HSE_ON;
RCC_OscInitStruct.HSIState = RCC_HSI_ON;
RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
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 = 12;
RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
RCC_OscInitStruct.PLL.PLLQ = RCC_PLLQ_DIV2;
RCC_OscInitStruct.PLL.PLLR = RCC_PLLR_DIV2;
if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) {
// Error_Handler();
}
/** 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.SYSCLKSource = RCC_SYSCLKSOURCE_HSI;
RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV1;
RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;
if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_0) != HAL_OK) {
// Error_Handler();
}
}
static void MX_USB_PCD_Init(void)
{
/* USER CODE BEGIN USB_Init 0 */
/* USER CODE END USB_Init 0 */
/* USER CODE BEGIN USB_Init 1 */
/* USER CODE END USB_Init 1 /
hpcd_USB_FS.Instance = USB;
hpcd_USB_FS.Init.dev_endpoints = 8;
hpcd_USB_FS.Init.speed = PCD_SPEED_FULL;
hpcd_USB_FS.Init.phy_itface = PCD_PHY_EMBEDDED;
hpcd_USB_FS.Init.Sof_enable = DISABLE;
hpcd_USB_FS.Init.low_power_enable = DISABLE;
hpcd_USB_FS.Init.lpm_enable = DISABLE;
hpcd_USB_FS.Init.battery_charging_enable = DISABLE;
if (HAL_PCD_Init(&hpcd_USB_FS) != HAL_OK)
{
// Error_Handler();
}
/ USER CODE BEGIN USB_Init 2 /
HAL_PCD_Start(&hpcd_USB_FS);
/ USER CODE END USB_Init 2 */
}
static void MX_GPIO_Init(void)
{
__HAL_RCC_GPIOF_CLK_ENABLE();
__HAL_RCC_GPIOA_CLK_ENABLE();
}