BLDC velocity control with gyroscope

[VikSauce]

Hello, I am trying to make a self balancing reaction wheel, and I am running into issues when it comes to using the output of my gyroscope to adjust the speed of my motor.

Here is the hardware I am using:

• Nucleo 64 stm32 G474RE
• SimpleFOC Drive v1.3
• 24V power supply (2 LiPo 12V batteries in series)
• BLDC 42BLS40-24-01 motor from StepperOnline
• MPU 6050 Gyroscope

I got both closed loop velocity control of the motor and the gyro sensor working on their own, but when I try to use the sensor output to adjust the speed of the motor, the code does not work, and the motor gets stuck on the hall sensor alignment step and heats up rapidly, which was never a problem during closed loop testing.
Here is the code:

#include <SimpleFOC.h>
#include <MPU6050.h>
#include <Wire.h>
#include <HardwareTimer.h>

// ── BLDC motor & driver instance ─────────────────────────────
BLDCMotor motor = BLDCMotor(4);
BLDCDriver3PWM driver = BLDCDriver3PWM(D6, D10, D5, D8);

// ── HallSensor ───────────────────────────────────────────────
HallSensor sensor = HallSensor(PC6, PB7, PB8, 4);
void doH1() { sensor.handleA(); }
void doH2() { sensor.handleB(); }
void doH3() { sensor.handleC(); }

// ── MPU6050 ──────────────────────────────────────────────────
MPU6050 imu;

// ── Gyro parameters ──────────────────────────────────────────
#define GYRO_SCALE      0.05f  // motor rad/s per gyro deg/s
#define MAX_MOTOR_SPEED 30.0f  // rad/s
#define DEADZONE        5.0f   // deg/s
#define GYRO_LPF_TF     0.1f   // seconds — gyro smoothing

// ── IMU timer interrupt state ─────────────────────────────────
volatile float imu_target = 0;
float filtered_gyro = 0;
float bias_gx = 0;
float bias_gy = 0;
float bias_gz = 0;
HardwareTimer* imuTimer;

// ── Commander ────────────────────────────────────────────────
Commander command = Commander(Serial);
void doMotor(char* cmd) { command.motor(&motor, cmd); }

// ── Gyro calibration ─────────────────────────────────────────
#define CALIBRATION_SAMPLES 500

void calibrateGyro() {
  Serial.println("Calibrating gyro — keep sensor still...");
  float sum_gx = 0, sum_gy = 0, sum_gz = 0;
  for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
    int16_t ax, ay, az, gx, gy, gz;
    imu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    sum_gx += gx / 65.5f;
    sum_gy += gy / 65.5f;
    sum_gz += gz / 65.5f;
    delay(2);
  }
  bias_gx = sum_gx / CALIBRATION_SAMPLES;
  bias_gy = sum_gy / CALIBRATION_SAMPLES;
  bias_gz = sum_gz / CALIBRATION_SAMPLES;

  Serial.print("Bias — gx: "); Serial.print(bias_gx);
  Serial.print("  gy: ");      Serial.print(bias_gy);
  Serial.print("  gz: ");      Serial.println(bias_gz);
  Serial.println("Calibration done.");
}

// ── IMU timer interrupt — runs at 100Hz ──────────────────────
void readIMU() {
  int16_t gx, gy, gz;
  imu.getRotation(&gx, &gy, &gz);

  // Subtract bias and convert to deg/s
  float gyro_dps = (gz / 65.5f) - bias_gz;

  // Deadzone
  if (abs(gyro_dps) < DEADZONE) gyro_dps = 0;

  // Low-pass filter
  float alpha = 0.01f / (GYRO_LPF_TF + 0.01f);
  filtered_gyro += alpha * (gyro_dps - filtered_gyro);

  // Map to motor target and clamp
  imu_target = constrain(filtered_gyro * GYRO_SCALE,
                         -MAX_MOTOR_SPEED, MAX_MOTOR_SPEED);
}

void setup() {
  // use monitoring with serial
  Serial.begin(115200);
  // enable more verbose output for debugging
  // comment out if not needed
  SimpleFOCDebug::enable(&Serial);

  // ── MPU6050 init ───────────────────────────────────────────
  Wire.setSDA(PA8);
  Wire.setSCL(PA9);
  Wire.begin();
  imu.initialize();
  if (!imu.testConnection()) {
    Serial.println("MPU6050 connection failed! Check wiring.");
    while (1);
  }
  Serial.println("MPU6050 OK");
  imu.setFullScaleGyroRange(MPU6050_GYRO_FS_500);

  // Calibrate 
  calibrateGyro();

  // ── IMU timer — 
  imuTimer = new HardwareTimer(TIM6);
  imuTimer->setOverflow(100, HERTZ_FORMAT);
  imuTimer->attachInterrupt(readIMU);
  imuTimer->resume();

  // ── Hall sensor ────────────────────────────────────────────
  sensor.init();
  sensor.enableInterrupts(doH1, doH2, doH3);
  motor.linkSensor(&sensor);

  // ── Driver ─────────────────────────────────────────────────
  driver.voltage_limit = 12;
  if (!driver.init()) {
    Serial.println("Driver init failed!");
    return;
  }
  motor.linkDriver(&driver);
  motor.voltage_limit = 2;
  motor.voltage_sensor_align = 1.5;
  motor.torque_controller = TorqueControlType::voltage;
  motor.controller = MotionControlType::velocity;

  float max_velocity = 100.0;
  float motor_frequency_hz = max_velocity / (2 * PI);
  motor.LPF_velocity.Tf = 0.15;
  motor.PID_velocity.P = 0.15;
  motor.PID_velocity.I = 0.35;
  motor.PID_velocity.D = 0;

  // comment out if not needed
  motor.useMonitoring(Serial);

  if (!motor.init()) {
    Serial.println("Motor init failed!");
    return;
  }
  if (!motor.initFOC()) {
    Serial.println("FOC init failed!");
    return;
  }

  motor.target = 0;
  command.add('M', doMotor, "Motor");
  Serial.println("Motor ready.");
  _delay(1000);
}

void loop() {
  // main FOC algorithm function
  motor.loopFOC();
  // Motion control function
  motor.move();
  // user communication
  command.run();
  motor.monitor();

  motor.target = imu_target;
}

Any help is greatly appreciated!

Edit: I tried to adapt the reaction wheel example provided on the simpleFOC website, but I didn’t manage to get something working

[o_lampe]

@VikSauce

imuTimer->resume();

What, if you put that line behind motor.initFOC

I also checked for interrupt conficts between TIM6 and the hall sensor pins, but they’re OK

I didn’t check I2C pins, PA8 and PB8 might clash, but does I2C-SDA occupy an interrupt? IDK