Position control period

Hello everyone! I want to use the position control mode of simpFOC to control a three-axis gimbal. The data return period of the IMU I use is 5 ms, so I send simpleFOC a target position command every 5 ms. But now I find that when I send the same position command every 5ms, the motor shakes at a very low frequency(about 0.5HZ). I tried to change the PID, but it didn’t work. So I want to know whether simpleFOC can complete the control task within 5 ms?
Besides, this is my code:

#include <SimpleFOC.h>

#define Motor_No 'P'
#define volt_limit 8.0000

MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C);
BLDCMotor motor = BLDCMotor(11);    
BLDCDriver3PWM driver = BLDCDriver3PWM(3, 5, 6, 8);     

float target_angle = 0;
float received_angle = 0;   

void setup() {
  driver.voltage_power_supply = 12;
  motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
  motor.controller = MotionControlType::angle;
  motor.PID_velocity.P = 0.2;
  motor.PID_velocity.I = 3;
  motor.PID_velocity.D = 0;
  motor.P_angle.P = 3;
  motor.voltage_limit = 8;
  motor.velocity_limit = 20;
  motor.LPF_velocity.Tf = 0.01;

void loop() {

void serialReceiveUserCommand() {
  static String received_chars;
  while (Serial.available()) {
    char inChar = (char)Serial.read();
    if (inChar == Motor_No)
      while (1)
        inChar = (char)Serial.read();
        if (inChar == ',')break;
        received_chars += inChar;
      received_angle = received_chars.toFloat();
      target_angle = received_angle;
      received_chars = "";


You don’t mention which MCU you use? How quickly the main loop will run will depend on your MCU speed. On a Arduino Nano it might be around 1ms. On a ESP32 it will be considerably faster.

5ms is 200Hz, that’s an ok control speed I would have said - fast enough for pretty dynamic control, but not so fast as to overload the MCU. But there are people here who know much more about such things and may have a better opinion for you.

Do you notice a behaviour difference if you slow down the control messages to 50ms, or 500ms? Really, if you are sending the same position, it should not make a difference how often you send it.

My assumption would be that you have some other problem .e.g. maybe the AS5600 (it is not a very exact sensor) is not that stable… I assume the IMU is not in the control loop in the test you are doing?

I would also code the Serial loop differently - if you do it in the way you posted then you run the risk of reading 1 byte, and then busy-waiting until the next byte arrives. Since serial has no real-time guarantees this could be a potentially long time. I would structure the loop differently, maybe more like this:

 while (Serial.available()) {
    char inChar == (char)Serial.read();
    if (inChar == ',') {
       target_angle = received_chars.toFloat();
       received_chars = "";
    else if (inChar != Motor_No)
        received_chars += inChar;

Probaly the root cause. I have a stupid suggestion perhaps, to debug, comment out the serial calls completely from the loop, and hard-code a full circle with very close positions with 5ms delay and see if it still vibrates.

Thanks for your reply! Sorry, I didn’t describe the details clearly.
This is my hardware:
MCU: Atmaga328P, magnetic encoder: AS5600.
My operation is to use the serial port host computer to send the same position to simpleFOC every 5ms, and it will shake.
I tried other control periods, such as 50ms, 500 ms and 1s. The fact is that only when I send a command every 1 second or
more, the motor won’t shake. I also think it is a problem with my serial port, I will try your suggestions later.
Thanks again!

I think it is also a good suggestion and I will try it.

Amazing! I tried to change the serial port part into your code and it worked! Thank you very much!

1 Like

Hey, I’m happy! The reason then was the delays caused by the serial communications…