Motor works with Arduino Uno but not with STM32 Nucleo-64 (DRV8301, velocity_openloop)

Greetings all,

Hope everyone is doing well.

I have been using (learning) the simpleFOC library implementation and I was able to run the 24V BLDC motor I have. It’s a basic setup with Arduino Uno and a DRV8301 (ZonRi, AliExpress) driver board. I didn’t set the pwm_frequency variable for the driver, and I was getting PWM at frequency 5kHz. This is in velocity_openloop mode. I was able to run the motor in position_openloop modeand hall-based closed-loop in torque, voltage mode.

Somehow, I ended up damaging my D3(~) and D4 pins. Also, current-sensing hasn’t been implemented for Arduino so I had to move on anyway, and try the STM32 Nucleo-64 that I have (F410RB).

First few tries:
GVDD_UV fault on the DRV8301. Had also chosen TIM5 for PWM and got PWM frequency of around 400 Hz. Realized it can’t give frequency high enough (maybe it can) for motor control and that could be causing the issue. I just moved on and chose pins connected to TIM1

With TIM1:

  1. PWMs still had similar low frequency. Set the driver.pwm_frequency variable at 20,000 and got around 6.25 kHz of pwm, Increased it to 40,000 and got around 8.333 kHz.
  2. GVDD_UV fault: This was happening cause for some reason the DRV8301 is drawing too much current. Solution: Increased the current limit on the power supply. The power supply goes into current control mode with current reaching the max set limit and voltage dropping from 24V to as low as 8V. And the motor just shakes - oscillates with a very tiny angular displacement. You can barely notice it. But you can easily feel the heat, the motor gets really hot in under a minute.

Now, the PWM waveform being generated by the Nucleo is also a little weird looking. While the ones generated by the Arduino very even (almost 50% duty), the ones from the Nucleo are not symmetric. I tried a Bluepill (F103C8T6) as well, to generate the PWMs (didn’t connect the driver/motor) and got similar PWM waveforms.

As a new user, I am unable to add more media (images) to my post. So I will add the waveform snapshots from my logic analyser as replies to this post.

The same firmware is being flashed on the three boards. Making sure to change the pins accordingly and have added the pwm_driver.pwm_frequency line for the STM32s:

#include <SimpleFOC.h>
#include <DRV8301.h>

// #define INH_A PA_0  // TIM5
// #define INH_B PA_1  // TIM5
// #define INH_C PB_11 // TIM5

#define INH_A PA_10 // TIM1 D2
#define INH_B PA_8  // TIM1 D7
#define INH_C PA_9  // TIM1 D8

#define EN_GATE PB_5
#define nFAULT  PC_7   

#define SCLK  PA_5  // SPI Sync Clock
#define nSCS  PB_6  // SPI Chip Select (SS - Slave Select)
#define SDI   PA_7  // Serial Data Input  (SPI MOSI)
#define SDO   PA_6  // Serial Data Output (SPI MISO)

// BLDCMotor(int pp,  float R = NOT_SET, float KV = NOT_SET, float L = NOT_SET);
// for Delta connection, R_phase = R_line * 1.5 = 0.8 * 1.5
// Bemf Constant = 2.71 Vrms/ KRPM 
// KV = 1/K_Bemf = 1000/2.71 = 369 RPM/V + 10% = 405.9 RPM/V
BLDCMotor motor = BLDCMotor(4, 1.2, 405.9);

BLDCDriver3PWM pwm_driver = BLDCDriver3PWM(INH_A, INH_B, INH_C);
// DRV8301 gate_driver = DRV8301(MOSI, MISO, SCLK, CS, EN_GATE, FAULT);
DRV8301 gate_drv = DRV8301(SDI, SDO, SCLK, nSCS, EN_GATE, nFAULT);

float target_velocity = 2; // [rad/s]

// utility function enabling serial communication with the user to set the target values
// this function can be implemented in serialEvent function as well
void serialReceiveUserCommand()
{
    // a string to hold incoming data
    static String received_chars;

    while (Serial.available())
    {
        // get the new byte:
        char inChar = (char)Serial.read();
        // add it to the string buffer:
        received_chars += inChar;
        // end of user input
        if (inChar == '\n')
        {
            // change the motor target
            target_velocity = received_chars.toFloat();
            Serial.print("Target velocity ");
            Serial.println(target_velocity);

            // reset the command buffer
            received_chars = "";
        }
    }
}

void setup()
{
    Serial.begin(115200);
    SimpleFOCDebug::enable(&Serial);
    
    // power supply voltage [V]
    pwm_driver.pwm_frequency = 40000;
    pwm_driver.voltage_power_supply = 24;
    pwm_driver.init();
    // pwm_driver.enable();

    gate_drv.begin(PWM_INPUT_MODE_3PWM);
   
    
    motor.linkDriver(&pwm_driver);    // link the motor and the driver

    // limiting motor movements
    motor.voltage_limit = 1;   // [V]
    motor.current_limit = 2;
    // motor.velocity_limit = 20; // [rad/s]

    // open loop control config
    motor.controller = MotionControlType::velocity_openloop;

    motor.init();

    Serial.println("Motor ready!");
    _delay(1000);
}

void loop()
{
    // open loop velocity movement
    // using motor.voltage_limit and motor.velocity_limit
    motor.move(target_velocity);

    int allFaults = gate_drv.read_fault();
    Serial.println(allFaults);

    // receive the used commands from serial
    serialReceiveUserCommand();
}

Please help me in figuring out the problem here.

Why are the STM32 boards unable to make the DRV8301, drive the motor?
Also, why is the PWM frequency not increasing according the code? At 20,000, I barely get 6,000.

Thank you for taking the time to read. Feel free to leave any suggestions you have or doubts regarding the setup, I will be happy to explain.

Warm Regards

Hi @elemoman,

I’ve just tested the library with the nucleo f410rb (the new release of the library). I wanted to check because the f410rb was recently added to the stm32duino and I was thinking that it might have some clock configuration that is wrong.

But I have exactly the same output as you

TIM1-CH3 TIM1-CH1 TIM1-CH2 score: 1
STM32-DRV: best: TIM1-CH3 TIM1-CH1 TIM1-CH2 score: 1
STM32-DRV: Initializing TIM1
STM32-DRV: Timer resolution set to: 1250
STM32-DRV: Configured TIM1_CH3
STM32-DRV: Configured TIM1_CH1
STM32-DRV: Configured TIM1_CH2
STM32-DRV: Synchronising 1 timers
MOT: Init
MOT: Enable driver.
Motor ready!

However I also have a 40kHz center-aligned PWM signal comming out of the pins 2,7 and 8.

Which library version are you using?

Here is the code I ran
#include <SimpleFOC.h>
// #include <DRV8301.h>

// #define INH_A PA_0  // TIM5
// #define INH_B PA_1  // TIM5
// #define INH_C PB_11 // TIM5

#define INH_A PA_10 // TIM1 D2
#define INH_B PA_8  // TIM1 D7
#define INH_C PA_9  // TIM1 D8

#define EN_GATE PB_5
#define nFAULT  PC_7   

#define SCLK  PA_5  // SPI Sync Clock
#define nSCS  PB_6  // SPI Chip Select (SS - Slave Select)
#define SDI   PA_7  // Serial Data Input  (SPI MOSI)
#define SDO   PA_6  // Serial Data Output (SPI MISO)

// BLDCMotor(int pp,  float R = NOT_SET, float KV = NOT_SET, float L = NOT_SET);
// for Delta connection, R_phase = R_line * 1.5 = 0.8 * 1.5
// Bemf Constant = 2.71 Vrms/ KRPM 
// KV = 1/K_Bemf = 1000/2.71 = 369 RPM/V + 10% = 405.9 RPM/V
BLDCMotor motor = BLDCMotor(4, 1.2, 405.9);

BLDCDriver3PWM pwm_driver = BLDCDriver3PWM(2, 7, 8);
// DRV8301 gate_driver = DRV8301(MOSI, MISO, SCLK, CS, EN_GATE, FAULT);
// DRV8301 gate_drv = DRV8301(SDI, SDO, SCLK, nSCS, EN_GATE, nFAULT);

float target_velocity = 2; // [rad/s]

// utility function enabling serial communication with the user to set the target values
// this function can be implemented in serialEvent function as well
void serialReceiveUserCommand()
{
    // a string to hold incoming data
    static String received_chars;

    while (Serial.available())
    {
        // get the new byte:
        char inChar = (char)Serial.read();
        // add it to the string buffer:
        received_chars += inChar;
        // end of user input
        if (inChar == '\n')
        {
            // change the motor target
            target_velocity = received_chars.toFloat();
            Serial.print("Target velocity ");
            Serial.println(target_velocity);

            // reset the command buffer
            received_chars = "";
        }
    }
}

void setup()
{
    Serial.begin(115200);
    SimpleFOCDebug::enable(&Serial);
    _delay(1000);
    
    // power supply voltage [V]
    pwm_driver.pwm_frequency = 40000;
    pwm_driver.voltage_power_supply = 24;
    pwm_driver.init();
    // pwm_driver.enable();

    //gate_drv.begin(PWM_INPUT_MODE_3PWM);
   
    
    motor.linkDriver(&pwm_driver);    // link the motor and the driver

    // limiting motor movements
    motor.voltage_limit = 1;   // [V]
    motor.current_limit = 2;
    // motor.velocity_limit = 20; // [rad/s]

    // open loop control config
    motor.controller = MotionControlType::velocity_openloop;

    motor.init();

    Serial.println("Motor ready!");
    _delay(1000);
}

void loop()
{
    // open loop velocity movement
    // using motor.voltage_limit and motor.velocity_limit
    motor.move(target_velocity);

   // int allFaults = gate_drv.read_fault();
    //Serial.println(allFaults);

    // receive the used commands from serial
    serialReceiveUserCommand();
}

EDIT:

Can it be that your scope is measuring the frequency wrong. The Arduino UNO should be at 32kHz by default, and the scope is measuring 6.25khz. Which is a factor or around 5.
In that case it makes sense that you measured 5kHz be default for the stm32 (5x5 = 25kHz) and when you changed to 40kHz you measured about 8 (8x5=40).

This part is not normal for the open-loop. And it seems once again like one phase is not connected or something :smiley:

This is normal, as you increase the voltage the motor is capable of going faster. This is expected.

This is also expected, at some point the voltage is not strong enough to beat the back-emf and the motor stalls. The higher the motor.volatge_limit the higher you’ll be able to go.

It makes sense too, the higher voltage the higher the current (current = voltage/phase_resistance)

You have the same behavior with the bluepill right?
The nucleo F410RB might be the issue too. Its been added relatively recently to the stm32duino (by me :grimacing: ) so there might be some issues happening due to its low-level configuration that I’ve done wrong.
I’d suggest you to use the bluepill, as it has been used in many projects so far and we are sure that it works. :slight_smile:

Arduino PWM waveform:

Nucleo PWM waveform:

Bluepill PWM waveform:

Hi @elemoman,

Something is definitely wrong… the PWM frequency on STM32 should be 25kHz by default.

could you share the debug output from the serial console? It should print infos about which timers are being used…

Hi Richard @runger,

Thanks for replying!

Here’s the debug output from the serial console, I just flashed the Nucleo with pwm_driver.pwm_frequency = 40000;:

13:52:09.145 → 20TIM1-CH3 TIM1-CH1 TIM1-CH2 score: 1
13:52:09.145 → STM32-DRV: best: TIM1-CH3 TIM1-CH1 TIM1-CH2 score: 1
13:52:09.145 → STM32-DRV: Initializing TIM1
13:52:09.145 → STM32-DRV: Timer resolution set to: 1250
13:52:09.145 → STM32-DRV: Configured TIM1_CH3
13:52:09.145 → STM32-DRV: Configured TIM1_CH1
13:52:09.145 → STM32-DRV: Configured TIM1_CH2
13:52:09.145 → STM32-DRV: Synchronising 1 timers
13:52:09.145 → MOT: Init
13:52:09.145 → MOT: Enable driver.
13:52:09.145 → Motor ready!
13:52:45.743 → TIM1-CH3 TIM1-CH1 TIM1-CH2 score: 1
13:52:45.743 → STM32-DRV: best: TIM1-CH3 TIM1-CH1 TIM1-CH2 score: 1
13:52:45.743 → STM32-DRV: Initializing TIM1
13:52:45.743 → STM32-DRV: Timer resolution set to: 1250
13:52:45.743 → STM32-DRV: Configured TIM1_CH3
13:52:45.743 → STM32-DRV: Configured TIM1_CH1
13:52:45.743 → STM32-DRV: Configured TIM1_CH2
13:52:45.743 → STM32-DRV: Synchronising 1 timers
13:52:45.743 → MOT: Init
13:52:45.743 → MOT: Enable driver.
13:52:45.743 → Motor ready!

The PWM frequency is at 8.333 kHz.

It does say timer resolution set to 1250. Hmmmmm.

Hello @Antun_Skuric

Yep, you are correct. I was, um, well, sampling at 25 kS/s … :neutral_face:

PWM frequency from the Nucleo is at 40 kHz.

This is great, and embarassing. So the PWM frequency is not the problem… but the DRV8301 is still sinking a lot of current with the Nucleo. Let me rewire the setup again, from scratch. This time with the Bluepill and share the results.

Thank you for the time and patience y’all :man_bowing: :man_bowing: :man_bowing:

And Arduino, as you mentioned, by default is at 31.25 kHz:

So I tried controlling the driver with both the Nucleo and the Bluepill. Same results. It just oscillates. Each of the motor phase connections are correct and fine (it does work with the Arduino Uno).

I changed the frequency in the program to 32000 Hz, as is for Arduino but it shows the same behaviour. I tried to upload a video but it won’t let me. Here’s a google drive link.

I doubt it helps but here’s an image of the setup:

The code for the bluepill:
#include <SimpleFOC.h>
#include <DRV8301.h>

#define INH_A PA8   // TIM1 Ch1
#define INH_B PA9   // TIM1 Ch2 
#define INH_C PA10  // TIM1 Ch3

#define EN_GATE PB14 
#define nFAULT  PB13   

#define SCLK  PB3   // SCK1: SPI Sync Clock
#define nSCS  PA15  // NSS1: SPI Chip Select (SS - Slave Select)
#define SDI   PB5   // Serial Data Input  (SPI MOSI)
#define SDO   PB4   // Serial Data Output (SPI MISO)
     
BLDCMotor motor = BLDCMotor(4, 1.2, 405.9);

BLDCDriver3PWM pwm_driver = BLDCDriver3PWM(INH_A, INH_B, INH_C);
// DRV8301 gate_driver = DRV8301(MOSI, MISO, SCLK, CS, EN_GATE, FAULT);
DRV8301 gate_drv = DRV8301(SDI, SDO, SCLK, nSCS, EN_GATE, nFAULT);

float target_velocity = 2; // [rad/s]

void serialReceiveUserCommand()
{
    static String received_chars;

    while (Serial.available())
    {
        char inChar = (char)Serial.read();
        received_chars += inChar;
        if (inChar == '\n')
        {
            target_velocity = received_chars.toFloat();
            Serial.print("Target velocity ");
            Serial.println(target_velocity);
            received_chars = "";
        }
    }
}

void setup()
{
    Serial.begin(115200);
    SimpleFOCDebug::enable(&Serial);
    
    // driver config
    // power supply voltage [V]
    pwm_driver.pwm_frequency = 32000;
    pwm_driver.voltage_power_supply = 24;
    pwm_driver.init();

    gate_drv.begin(PWM_INPUT_MODE_3PWM);
   
    
    motor.linkDriver(&pwm_driver);

    // limiting motor movements
    motor.voltage_limit = 1;   // [V]
    motor.current_limit = 2;
    // motor.velocity_limit = 20; // [rad/s]

    // open loop control config
    motor.controller = MotionControlType::velocity_openloop;

    // init motor hardware
    motor.init();

    Serial.println("Motor ready!");
    _delay(1000);
}

void loop()
{
    motor.move(target_velocity);
    serialReceiveUserCommand();
}

Hey, great news, so PWM is working in principle.

40kHz may be a little fast, and might explain some of the heat you’re noticing.

Perhaps at 1.2 Ohm and only 1V limit the currents are too low? What kind of motor is it? If you increase the limits, pls do so slowly.

Is the driver fault coming on?

Hey Richard @runger

Yes, the PWM has been working just fine - the sampling frequency of my scope was the problem.

40 kHz could be fast yes, but Arduino 32 kHz seem to be working just fine. I even programmed it at 32 kHz and that gave the same results. I think it may be something to do with the shape of the PWM pulses. Arduino PWM is symmetric with the 50% duty wheres the STM32’s are given a rather variable duty.

I have shared the motor datasheet, it’s a BLDC. However, I have checked the BEMF, and it’s more sinusoidal than trapezoidal. The motor is rated for 0.5 A at no load, and given the rated torque at 0.125 Nm, the current at rated torque would be T_r / K_t = 0.125 / 0.0355 = 3.5 A.

So the currents are not too low, they are actually on the higher side. The DC side current is being drawn to the max from the power supply. With the Arduino Uno control, the motor runs and the current goes max up to 0.15 A.

I am not planning to increase the limits, not yet; I may just try out the current-closed loop control now cuz STM32s just don’t seem to be working out.

No driver faults, there was the GVDD_UV but that’s only with STM32’s and occurs if I set the current limit too low at 2 A. It makes sense, cuz the driver wants to draw a lot of power and when it tries to draw more current, the power will remain constant on the power supply end and this will cause the voltage to go down. With a high enough current limit, i.e. 3 A or so, it doesn’t show the under-voltage fault.

Very stumped lol.

Thanks for reading and the responses!
Warm regards

Update:

Tried closed-loop (hall-sensor) torque control (voltage mode) and it’s unable to even calibrate the sensors in the initialization, the same vibrations/oscillations happen and the firmware throws “motor init failed”.

Same with the closed-loop (hall-sensor, current-sense) torque control (dc-current mode).

Did so with both the Nucleo and the Bluepill.

Code used:

torque:dc_current control with Nucleo-F410RB
#include <SimpleFOC.h>
#include <DRV8301.h>

#define IOUT_A PA_0      // A0
#define IOUT_B PA_1      // A1
#define IOUT_C PA_4      // A2

#define INH_A PA_10     // TIM1 | D2
#define INH_B PA_8      // TIM1 | D7
#define INH_C PA_9      // TIM1 | D8

#define nFAULT  PB_3    // D3
#define EN_GATE PB_5    // D4 

#define SCLK  PA_5      // D13 | SPI Sync Clock
#define nSCS  PB_6      // D10 | SPI Chip Select (SS - Slave Select)
#define SDI   PA_7      // D11 | Serial Data Input  (SPI MOSI)
#define SDO   PA_6      // D12 | Serial Data Output (SPI MISO)

#define HALL_A PB_12
#define HALL_B PC_10
#define HALL_C PC_11

BLDCMotor motor = BLDCMotor(4, 1.2, 405.9);
BLDCDriver3PWM pwm_driver = BLDCDriver3PWM(INH_A, INH_B, INH_C);
// DRV8301 gate_driver = DRV8301(MOSI, MISO, SCLK, CS, EN_GATE, FAULT);
DRV8301 gate_drv = DRV8301(SDI, SDO, SCLK, nSCS, EN_GATE, nFAULT);
HallSensor sensor = HallSensor(HALL_A, HALL_B, HALL_C, 4);

LowsideCurrentSense current_sense  = LowsideCurrentSense(0.005, 12.22, IOUT_A, IOUT_B, IOUT_C);

void setup() {
  Serial.begin(115200);
  SimpleFOCDebug::enable(&Serial);

  // driver config
  pwm_driver.pwm_frequency = 20000;
  pwm_driver.voltage_power_supply = 24;
  
  // driver init
  pwm_driver.init();

  // drv8301 
  gate_drv.begin(PWM_INPUT_MODE_3PWM);

  // link current sense and driver
  current_sense.linkDriver(&pwm_driver);

  motor.linkDriver(&pwm_driver);
  
  sensor.pullup = Pullup::USE_INTERN;
  sensor.init();
  
  Serial.println("Sensor ready");

  motor.linkSensor(&sensor);

  motor.voltage_limit = 24;
  motor.current_limit = 3;
  motor.controller = MotionControlType::torque;
  motor.torque_controller = TorqueControlType::dc_current;

  // foc current control parameters (Arduino UNO/Mega)
  motor.PID_current_q.P = 5;
  motor.PID_current_q.I= 300;
  motor.LPF_current_q.Tf = 0.01; 
  
  motor.init();
  
  // init current sense
  current_sense.init();
  motor.linkCurrentSense(&current_sense);

  motor.initFOC();

  _delay(1000);
}

float target_current = 0;

void loop() {
  motor.loopFOC();
  
  motor.move(target_current);
  
  DQCurrent_s current = current_sense.getFOCCurrents(4*sensor.getAngle());
  Serial.print(current.d);
  Serial.print("\t");
  Serial.print(current.q);
  Serial.print("\t");
  Serial.println(sensor.getVelocity());
  serialReceiveUserCommand();
}

// utility function enabling serial communication with the user to set the target values
// this function can be implemented in serialEvent function as well
void serialReceiveUserCommand()
{
    // a string to hold incoming data
    static String received_chars;

    while (Serial.available())
    {
        // get the new byte:
        char inChar = (char)Serial.read();
        // add it to the string buffer:
        received_chars += inChar;
        // end of user input
        if (inChar == '\n')
        {
            // change the motor target
            target_current = received_chars.toFloat();
            Serial.print("Target current ");
            Serial.println(target_current);

            // reset the command buffer
            received_chars = "";
        }
    }
}
torque:voltage control with Bluepill (STM32F103C8T6)
#include <SimpleFOC.h>
#include <DRV8301.h>

// #include <PciManager.h>     // for hall-sensor software interrupt setup
// #include <PciListenerImp.h> // for hall-sensor software interrupt setup

#define INH_A PA8   // TIM1 Ch1
#define INH_B PA9   // TIM1 Ch2 
#define INH_C PA10  // TIM1 Ch3

#define nFAULT  PB13
#define EN_GATE PB14

#define SCLK  PB3   // SCK1: SPI Sync Clock
#define nSCS  PA15  // NSS1: SPI Chip Select (SS - Slave Select)
#define SDI   PB5   // Serial Data Input  (SPI MOSI)
#define SDO   PB4   // Serial Data Output (SPI MISO)

#define HALL_A PA6
#define HALL_B PA7
#define HALL_C PB0

BLDCMotor motor = BLDCMotor(4, 1.2, 405.9);
BLDCDriver3PWM pwm_driver = BLDCDriver3PWM(INH_A, INH_B, INH_C);
// DRV8301 gate_driver = DRV8301(MOSI, MISO, SCLK, CS, EN_GATE, FAULT);
DRV8301 gate_drv = DRV8301(SDI, SDO, SCLK, nSCS, EN_GATE, nFAULT);
HallSensor sensor = HallSensor(HALL_A, HALL_B, HALL_C, 4);

// A, B and C interrupt callback buffers
// void doA(){sensor.handleA();}
// void doB(){sensor.handleB();}
// void doC(){sensor.handleC();}

// sensor interrupt init
// PciListenerImp listenA(sensor.pinA, doA);
// PciListenerImp listenB(sensor.pinB, doB);
// PciListenerImp listenC(sensor.pinC, doC);

void setup() {
    Serial.begin(115200);
    SimpleFOCDebug::enable(&Serial);

    pwm_driver.pwm_frequency = 20000;
    pwm_driver.voltage_power_supply = 24;
    pwm_driver.init();
    gate_drv.reset();
    gate_drv.begin(PWM_INPUT_MODE_3PWM);

    motor.linkDriver(&pwm_driver);

    sensor.pullup = Pullup::USE_INTERN;
    sensor.init();

    // interrupt initialization
    //   PciManager.registerListener(&listenA);
    //   PciManager.registerListener(&listenB);
    //   PciManager.registerListener(&listenC);
    Serial.println("Sensor ready");

    motor.linkSensor(&sensor);

    motor.voltage_limit = 1;
    motor.controller = MotionControlType::torque;
    motor.torque_controller = TorqueControlType::voltage;

    motor.init();
    motor.initFOC();

    _delay(1000);
}

float target_current = 0;

void loop() {
    motor.loopFOC();
    
    motor.move(target_current);
    
    //  Serial.print(sensor.getAngle());
    //  Serial.print("\t");
    //  Serial.println(sensor.getVelocity());
    serialReceiveUserCommand();
}

// utility function enabling serial communication with the user to set the target values
// this function can be implemented in serialEvent function as well
void serialReceiveUserCommand()
{
    // a string to hold incoming data
    static String received_chars;

    while (Serial.available())
    {
        // get the new byte:
        char inChar = (char)Serial.read();
        // add it to the string buffer:
        received_chars += inChar;
        // end of user input
        if (inChar == '\n')
        {
            // change the motor target
            target_current = received_chars.toFloat();
            Serial.print("Target current ");
            Serial.println(target_current);

            // reset the command buffer
            received_chars = "";
        }
    }
}Reg

Thanks and Regards!

Try using only the pole pairs number:

BLDCMotor motor = BLDCMotor(4);

The phase resistance and the KV rating are a bit more fiddly to get right.

Also, there is no real point in going closed-loop if the open-loop does not work. The closed loop uses the same low-level code and it only adds the complexity to the open-loop. So I’d suggest you to stay with the closed loop until you get the motor spinning properly. :smiley:

@Antun_Skuric

Oh yea it had occurred to me and I so I did try with just the pole-pairs, it didn’t work. Similar oscillations.

And yea about the closed loop, yeah you are right - I thought maybe by giving the controller more information, it could work :clown_face:. Open-loop control is supposed to be “bad”, so to speak.

The only remaining thing that I can think of is that the driver is not being configured well using the SPI. Can you try to read some driver parameters and see if the values are correct?

Or maybe you could try running it in the 6PWM mode, that way you will not need to configure the driver.

Hey @Antun_Skuric

I just did that. Both of the DRV8301’s control registers have same bits set, whether I use the Nucleo or Arduino.

Control Register 1: 5880 (0x16F8 or 1 0110 1111 1000)

Control Register 2: 6144 (0x1800 or 1 1000 0000 0000)

For the Arduino, as the motor spins, the register values do change a couple times, but stay mostly at 5880 and 6144:

Serial output (Arduino Uno):
5880	6144
5120	-28672
5880	6144
5880	6144
-2048	6144
5880	6144
5880	6144
5880	6144
5880	6144
5880	6144
5880	6144
5880	6144
5880	6144
5880	6144
5880	6144
5880	6144
5880	6144
5880	6144
5632	0
5880	6144
5880	6144
5880	6144
0	0
5880	6144
5880	6144
5880	6144
5880	6144
5880	6144
5880	6144
-2048	-16384
5880	6144
-4096	-2048
16515	-24544
5880	6144
5880	6144
5880	6144
5880	6144
5880	6144
5880	6144
5880	6144
8242	22572
5880	6144
5880	6144

Here are the register descriptions for easy reference.

Page 22/41, DRV8301 Datasheet: Link

I don’t see anything weird with the current register values, but it’s my first time doing something like this. Maybe you will catch something I missed. And frustratingly enough, they are the same for the Nucleo as well 5880 and 6144.

Gonna try 6PWM, I tried that quiet a while ago with the Arduino, and I was getting shoot-through fault with that. Something to do with Arduino being unable to create enough dead-time at high-frequency. I guess the STM32 is more capable and can probably take care of that. I’ll update!

Tried the 6PWM mode and it worked but there are a few quirks.

  • Speed command 1-6: Oscillates back and forth
  • Speed 7 - 16 : Rotates with jerks (at each pole I guess?).
  • Speed 17 - 22: Becomes smooth(er)
  • Speed > 22 : Oscillates back and forth, as in 1-6 but with higher frequency and it increases as I increase the speed command
  • Currents are rather low - my DC power supply shows 0.01-0.02 A being drawn at speed 22

Also if I directly jump from say 5-6 to 17-18, then it won’t rotate. It will just oscillate, but if I step up the speed command gradually, like 5-6 → 10-11 → 15-16, then it behaves as described above.

I increased the PWM frequency from 20k to 32k and found that the ranges shrink:

  • Speed 1-8 : Oscillates back and forth
  • 9 - 17 : Jerky rotations
  • 18 : You can call it smooth
  • 19 : Jerky rotations
  • Speed > 19 : Oscillates back and forth

I guess closing the loop could help now maybe?

I am still curious and want to put some theory behind these observations, it would be nice if someone can give explanations for this behaviour of the motor.

Here’s the code
#include <SimpleFOC.h>
#include <DRV8301.h>

#define INH_A PA_10     // TIM1_CH3 | D2
#define INH_B PA_8      // TIM1_CH1 | D7
#define INH_C PA_9      // TIM1_CH2 | D8

#define INL_A PB_1      // TIM1_CH3N | CN10 - 24
#define INL_B PB_13     // TIM1_CH1N | CN10 - 30
#define INL_C PB_0      // TIM1_CH2N | A3

#define EN_GATE PB_5    // D4 
#define nFAULT  PB_3    // D3

#define SCLK  PA_5      // D13 | SPI Sync Clock
#define nSCS  PB_6      // D10 | SPI Chip Select (SS - Slave Select)
#define SDI   PA_7      // D11 | Serial Data Input  (SPI MOSI)
#define SDO   PA_6      // D12 | Serial Data Output (SPI MISO)

BLDCMotor motor = BLDCMotor(4);

BLDCDriver6PWM pwm_driver = BLDCDriver6PWM(INH_A, INL_A, INH_B, INL_B, INH_C, INL_C, EN_GATE);
// DRV8301 gate_driver = DRV8301(MOSI, MISO, SCLK, CS, EN_GATE, FAULT);
DRV8301 gate_drv = DRV8301(SDI, SDO, SCLK, nSCS, EN_GATE, nFAULT);

float target_velocity = 2; // [rad/s]

// utility function enabling serial communication with the user to set the target values
// this function can be implemented in serialEvent function as well
void serialReceiveUserCommand()
{
    // a string to hold incoming data
    static String received_chars;

    while (Serial.available())
    {
        // get the new byte:
        char inChar = (char)Serial.read();
        // add it to the string buffer:
        received_chars += inChar;
        // end of user input
        if (inChar == '\n')
        {
            // change the motor target
            target_velocity = received_chars.toFloat();
            Serial.print("Target velocity ");
            Serial.println(target_velocity);

            // reset the command buffer
            received_chars = "";
        }
    }
}

void setup()
{
    Serial.begin(115200);
    SimpleFOCDebug::enable(&Serial);
    
    // driver config
    // power supply voltage [V]
    pwm_driver.pwm_frequency = 20000;
    pwm_driver.voltage_power_supply = 24;
    pwm_driver.init();

    gate_drv.begin(PWM_INPUT_MODE_6PWM);
   
    motor.linkDriver(&pwm_driver);    // link the motor and the driver

    // limiting motor movements
    motor.voltage_limit = 1;   // [V]
    // motor.current_limit = 2;
    // motor.velocity_limit = 20; // [rad/s]

    // open loop control config
    motor.controller = MotionControlType::velocity_openloop;

    // init motor hardware
    motor.init();

    Serial.println("Motor ready!");
    _delay(1000);

    Serial.println(gate_drv.drv8301_read_reg(DRV8301_CONTROL_REG1));
    Serial.println(gate_drv.drv8301_read_reg(DRV8301_CONTROL_REG2));
}

void loop()
{
    // open loop velocity movement
    // using motor.voltage_limit and motor.velocity_limit
    motor.move(target_velocity);
    Serial.print(gate_drv.drv8301_read_reg(DRV8301_CONTROL_REG1));
    Serial.print("\t");
    Serial.println(gate_drv.drv8301_read_reg(DRV8301_CONTROL_REG2));
    // int allFaults = gate_drv.read_fault();
    // Serial.println(allFaults);

    // receive the used commands from serial
    serialReceiveUserCommand();
}

Adding or removing the phase resistance and KV rating to the BLDCMotor object doesn’t change the behaviour at all.

Trying closed-loop torque, voltage control (with Hall-feedback) in 6PWM mode and upon giving current-commands I can see that the DC supply, does supply more current to the driver according to the command. But the motor doesn’t rotate.

At current-command 2 and above the rotor gets really tightly locked. You can’t easily move it with your hand.

Here’s how the DC supply is reacting to the serial current-commands:

Current Command DC Supply (A)
0.8 0.04
1 0.06
1.5 0.12
2 0.2
2.5 0.3
3 0.43
3.5 0.56
Current Command DC Supply (A)
4 0.71
4.5 0.88
5 1.07
5.5 1.26
6 1.46
6.5 1.66

At a current command of 10 (supply reaches current limit: 3A) :

Code
#include <SimpleFOC.h>
#include <DRV8301.h>

// #define IOUT_A PA_0      // A0
// #define IOUT_B PA_1      // A1
// #define IOUT_C PA_4      // A2

#define INH_A PA_10     // TIM1_CH3 | D2
#define INH_B PA_8      // TIM1_CH1 | D7
#define INH_C PA_9      // TIM1_CH2 | D8

#define INL_A PB_1      // TIM1_CH3N | CN10 - 24
#define INL_B PB_13     // TIM1_CH1N | CN10 - 30
#define INL_C PB_0      // TIM1_CH2N | A3

#define EN_GATE PB_5    // D4 
#define nFAULT  PB_3    // D3

#define SCLK  PA_5      // D13 | SPI Sync Clock
#define nSCS  PB_6      // D10 | SPI Chip Select (SS - Slave Select)
#define SDI   PA_7      // D11 | Serial Data Input  (SPI MOSI)
#define SDO   PA_6      // D12 | Serial Data Output (SPI MISO)

#define HALL_A PB_12
#define HALL_B PC_10
#define HALL_C PC_11

BLDCMotor motor = BLDCMotor(4, 1.2, 405.9);
BLDCDriver6PWM pwm_driver = BLDCDriver6PWM(INH_A, INL_A, INH_B, INL_B, INH_C, INL_C, EN_GATE);
// DRV8301 gate_driver = DRV8301(MOSI, MISO, SCLK, CS, EN_GATE, FAULT);
DRV8301 gate_drv = DRV8301(SDI, SDO, SCLK, nSCS, EN_GATE, nFAULT);
HallSensor sensor = HallSensor(HALL_A, HALL_B, HALL_C, 4);

float target_current = 2.0;

// utility function enabling serial communication with the user to set the target values
// this function can be implemented in serialEvent function as well
void serialReceiveUserCommand()
{
    // a string to hold incoming data
    static String received_chars;

    while (Serial.available())
    {
        // get the new byte:
        char inChar = (char)Serial.read();
        // add it to the string buffer:
        received_chars += inChar;
        // end of user input
        if (inChar == '\n')
        {
            // change the motor target
            target_current = received_chars.toFloat();
            Serial.print("Target current ");
            Serial.println(target_current);

            // reset the command buffer
            received_chars = "";
        }
    }
}

void setup() {
  Serial.begin(115200);
  SimpleFOCDebug::enable(&Serial);

  pwm_driver.pwm_frequency = 20000;
  pwm_driver.voltage_power_supply = 24;
  pwm_driver.init();
  gate_drv.begin(PWM_INPUT_MODE_6PWM);

  motor.linkDriver(&pwm_driver);
  Serial.println("Driver ready");

  sensor.pullup = Pullup::USE_INTERN; //IMPORTANT :SOB_EMOJI:
  sensor.init();
  motor.linkSensor(&sensor);
  Serial.println("Sensor ready");

  motor.voltage_limit = 24;
  // motor.current_limit = 3;
  motor.controller = MotionControlType::torque;
  motor.torque_controller = TorqueControlType::voltage;

  motor.init();
  motor.initFOC();
  Serial.println("Motor ready");

  _delay(1000);

  Serial.print(gate_drv.drv8301_read_reg(DRV8301_CONTROL_REG1));
  Serial.print("\t");
  Serial.println(gate_drv.drv8301_read_reg(DRV8301_CONTROL_REG2));
}

void loop() {
  motor.loopFOC();
  
  motor.move(target_current);
  
  // Serial.print(sensor.getSensorAngle());
  // Serial.print("\t");
  // Serial.println(sensor.getVelocity());

  serialReceiveUserCommand();
}