Hi,
I am trying to get the DRV8302 example to work with a STM32F405RTG6 based board.
I have been able to get the BLDCDriver3PWM to work, but when I test with the BLDCDriver6PWM by adding 3 additional pins it doesn’t work.
The 3 pins I add are PC6, PC7 and PC8 which are associated with the TIM8. It didn’t work. Tried swapping these pins for PA0, PA1, and PA2 which are related to the TIM5 … but haven’t been able to get it to work either.
The motor remains “dead”, unenergized. The hardware-level setup I don’t think contains any errors because if I run the example, for the BLDCDriver3PWM without touching a single cable, it works perfectly.
Does anyone know what combination of pins is needed for that setup with BLDCDriver6PWM?
Output:
MOT: Monitor enabled!
MOT: Initialise variables.
MOT: Enable driver.
MOT: Align sensor.
MOT: natural_direction==CW
MOT: Absolute zero align.
MOT: Success!
MOT: Motor ready.
platform.ini:
[env:genericSTM32F405RG]
platform = ststm32
board = genericSTM32F405RG
board_build.f_cpu = 168000000L
framework = arduino
debug_tool = stlink
upload_protocol = stlink
debug_init_cmds =
set CPUTAPID 0x2ba01477
target extended-remote $DEBUG_PORT
$INIT_BREAK
monitor reset halt
$LOAD_CMDS
monitor init
monitor reset halt
build_flags =
-D HSE_VALUE=8000000
-D HAVE_HWSERIAL1
-D PIN_WIRE_SDA=PB11
-D PIN_WIRE_SCL=PB10
-D PIN_SPI_MOSI=PA7
-D PIN_SPI_MISO=PA6
-D PIN_SPI_SCK=PA5
-D PIN_SPI_SS=PA4
-D HAL_CAN_MODULE_ENABLED
monitor_speed = 115200
lib_deps =
askuric/Simple FOC@^2.0.2
Wire
SPI
My code:
#include <SimpleFOC.h>
// This SystemClock_Config is copy&paste from CubeIDE
void SystemClock_Config(void)
{
RCC_OscInitTypeDef RCC_OscInitStruct = {0};
RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
// Configure the main internal regulator output voltage
//
__HAL_RCC_PWR_CLK_ENABLE();
__HAL_PWR_VOLTAGESCALING_CONFIG(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_OscInitStruct.HSIState = RCC_HSI_ON;
RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI;
RCC_OscInitStruct.PLL.PLLM = 8;
RCC_OscInitStruct.PLL.PLLN = 100;
RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
RCC_OscInitStruct.PLL.PLLQ = 4;
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_PLLCLK;
RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4;
RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2;
if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_3) != HAL_OK)
{
Error_Handler();
}
}
#define NUM_OF_BLINKS 15
void bootLedIndicator(){
int counter = NUM_OF_BLINKS;
pinMode(PA8, OUTPUT);
while(counter>0){
digitalToggle(PA8);
delay(50);
counter--;
}
}
// DRV8302 pins connections
// don't forget to connect the common ground pin
#define INH_A PB6 // Green
#define INH_B PB7 // Orange
#define INH_C PB8 // Blue
#define EN_GATE PB9 // Red
#define INL_A PC6 //PA0 // Purple
#define INL_B PC7 //PA1 // Gray
#define INL_C PC8 //PA2 // Yellow
#define M_PWM PB4 // White
#define M_OC PB5 // Brown
#define OC_ADJ PB3 // Black
#define MOSI PA7 // Blue
#define MISO PA6 // Green
#define CLK PA5 // Orange
#define CS PA4 // Yellow
MagneticSensorSPIConfig_s AS5047A_SPI_Config {
.spi_mode = SPI_MODE1,
.clock_speed = 10000000,
.bit_resolution = 14,
.angle_register = 0x3FFF,
.data_start_bit = 13,
.command_rw_bit = 14,
.command_parity_bit = 15
};
SPIClass spiConnection(MOSI,MISO,CLK);
// magnetic sensor instance - SPI
MagneticSensorSPI sensor = MagneticSensorSPI(AS5047A_SPI_Config, CS);
// Motor instance
BLDCMotor motor = BLDCMotor(12);
BLDCDriver6PWM driver = BLDCDriver6PWM(INH_A, INL_A, INH_B,INL_B, INH_C, INL_C, EN_GATE);
// utility function enabling serial communication the user
String serialReceiveUserCommand() {
// a string to hold incoming data
static String received_chars;
String command = "";
while (Serial1.available()) {
// get the new byte:
char inChar = (char)Serial1.read();
// add it to the string buffer:
received_chars += inChar;
// end of user input
if (inChar == '\n') {
// execute the user command
command = received_chars;
// reset the command buffer
received_chars = "";
}
}
return command;
}
double lastReport;
void setup() {
// initialise magnetic sensor hardware
sensor.init();
// link the motor to the sensor
motor.linkSensor(&sensor);
// link the motor to the sensor
// DRV8302 specific code
// M_OC - enable over-current protection
pinMode(M_OC,OUTPUT);
digitalWrite(M_OC,LOW);
// M_PWM - enable 6pwm mode (can be left open)
pinMode(M_PWM,OUTPUT);
digitalWrite(M_PWM,LOW);
// OD_ADJ - set the maximum over-current limit possible
// Better option would be to use voltage divisor to set exact value
pinMode(OC_ADJ,OUTPUT);
digitalWrite(OC_ADJ,HIGH);
// configure driver
driver.voltage_power_supply = 12;
driver.init();
motor.linkDriver(&driver);
// choose FOC modulation
motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
// set control loop type to be used
motor.controller = ControlType::angle;
// controller configuration based on the control type
motor.PID_velocity.P = 0.2;
motor.PID_velocity.I = 20;
// velocity low pass filtering time constant
motor.LPF_velocity.Tf = 0.2;
// angle loop controller
motor.P_angle.P = 20;
// angle loop velocity limit
motor.velocity_limit = 50;
// default voltage_power_supply
motor.voltage_limit = 0.90;
// use monitoring with serial for motor init
// monitoring port
Serial1.begin(115200);
// comment out if not needed
motor.useMonitoring(Serial1);
// initialize motor
motor.init();
// align encoder and start FOC
motor.initFOC();
// set the initial target value
motor.target = 2;
lastReport = millis();
bootLedIndicator();
_delay(1000);
}
void loop() {
// iterative setting FOC phase voltage
motor.loopFOC();
// iterative function setting the outer loop target
// velocity, position or voltage
// if target not set in parameter uses motor.target variable
motor.move();
double currentMillis = millis();
if(( currentMillis-lastReport)>20)
{
motor.monitor();
lastReport = currentMillis;
}
// user communication
motor.command(serialReceiveUserCommand());
}