Strange behaviour of system

Im trying to control motor speed by angle of 6050. But when motor starts moving and i to do big angle motor stop. When I move 6050 by big angle motor stops and void loop() stops to. And motor begins to warm up. What will be problem by this ?

#include <SimpleFOC.h>

#include <MPU6050_tockn.h>
#include <Wire.h>
MPU6050 mpu6050(Wire);

#define INH_A PB5
#define INH_B PB4
#define INH_C PB10
#define EN_GATE PB3
#define M_PWM PB13
#define M_OC PB14
#define OC_ADJ PA10

// Motor instance
BLDCMotor motor = BLDCMotor(7);
BLDCDriver3PWM driver = BLDCDriver3PWM(INH_A, INH_B, INH_C, EN_GATE);

// encoder instance
MagneticSensorSPI sensor = MagneticSensorSPI(AS5147_SPI, 10);

MPU6050 mpu(Wire);

// commander interface
Commander command = Commander(Serial);
void onMotor(char* cmd){ command.motor(&motor, cmd); }

void setup() {


Serial.print(“call end”);

// initialize encoder sensor hardware


// DRV8302 specific code
// M_OC - enable overcurrent protection
// M_PWM - enable 3pwm mode
// OD_ADJ - set the maximum overcurrent limit possible
// Better option would be to use voltage divisor to set exact value

// driver config
// power supply voltage [V]
driver.voltage_power_supply = 16;
// link the motor and the driver

// choose FOC modulation
motor.foc_modulation = FOCModulationType::SpaceVectorPWM;

// set control loop type to be used
motor.controller = MotionControlType::torque;

// contoller configuration based on the controll type
motor.PID_velocity.P = 0.2;
motor.PID_velocity.I = 20;
// default voltage_power_supply
motor.voltage_limit = 2;

// velocity low pass filtering time constant
motor.LPF_velocity.Tf = 0.01;

// angle loop controller
motor.P_angle.P = 20;
// angle loop velocity limit
motor.velocity_limit = 200;

// use monitoring with serial for motor init
// monitoring port

// comment out if not needed

// initialise motor
// align encoder and start FOC

// set the inital target value = 0.5;

command.add(‘M’, onMotor, “motor”);



void loop() {




What are the units of getAngleX() ? What kind of target values are you setting? Without setting the motor’s phase_resistance the target value is actually a voltage so your target values should stay relatively small…
Maybe you want MotionControlType::velocity instead?

Targets is angles from 0 to 90 but I have a prescaller 0.05 and its normal voltage value for my motor.
I don’t khow why my loop cycle stop in angle close 45* when motor moving very fast?

TBH, I can’t see the problem in the code.

If you’re only using 2V, you may want to lower your power supply. Using only 2V/16V, and then setting the speed within that 2V range limits your precision quite a bit.

Another concerns I would have is: how long is the MPU6050.update() taking? It should not take too much time.

I assume you have tested with setting just a constant torque (i.e. without the MPU input)? Have you tested velocity mode?
Have you printed your values, to make sure they make sense?

I dont understnd this. Can you explain to me more detail? Which voltage i need to try ?


Basically, when setting the voltage/current to the phases, the MCU uses PWM (pulse width modulation). In PWM, you set the voltage by setting the “duty cycle” - how long is the PWM signal “on”, how long is it “off”. The proportion of this time determines the voltage value.

So if your power supply is 16V, and your PWM duty-cycle is 50%, the resulting voltage set is 8V. If the duty cycle is 25%, the result is 4V…

The duty cycle on the MCU is set using a counter - this is an integer value which counts from 0 to N, where N is the “PWM resolution”. Typically N could be around 1000. So 50% duty cycle would be 500/1000 “ticks” of this counter.

So if your voltage limit is 2V/16V, and the counter resolution is 1000 (=16V), then your useable resolution is 125 (=2V) steps.

If (in this example) you set your power-supply to 5V, then 2V/5V = 400 steps, already considerably better…

Ok but I can to power the system by 16 volts?
Or I need to power 5 volts?

Look, this was just a suggestion, I don’t think this will actually solve your problem. But if you power the system (the motor) with a lower voltage, like 5V, you’d get a better resolution.

But to solve the problem I would focus more on the control/timing side:

  • does the motor turn well if you remove the MPU code and just turn it at a constant speed?
  • can you vary the speed without the MPU, e.g. using the commander like “M2.0”, “M50” ?
  • how long is the call to MPU.update() taking?

Is German your first language (I ask because of your username)? Wir können uns sehr gerne zu dem Thema auch auf Deutsch unterhalten…

Sind also tatsächlich deutsche hier.

So, probably is the problem from imu because now try real time speed/torque control by second encoder and it woks perfectly. I need to work more with library IMU and get an error.

void loop() {



Und Österreicher auch! :smiley: Servus!