Closed-loop position control issue: Spinning smoothly at lower velocities but transitions into discrete angles at higher velocities

hardware support
, ,
1

The issue is in the title, but specifically, when a target angle is input, the motor starts spinning correctly and smoothly to the target angle. When it reaches higher velocities, approximately above 50rad/s, the motor enters a phase of constant average rotational velocity, where it turns to discrete angles instead of continuously. In this phase, the driver and motor heat up as well.

I’ve tried experimenting on the pid values and other values and see that decreasing the P_angle helps somewhat, where theres less chance of it happening while adjusting other values don’t seem to help too much. This can’t be a fix, though, as it risks happening in the future.

Another thing is that when I tested open loop velocity, a lot of noise is being made above 50 rad/s so that is probably related.

this is the code, im testing with the motor without 1 following it. Original values are from mks dual foc example code

#include <SimpleFOC.h>


MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C);
MagneticSensorI2C sensor1 = MagneticSensorI2C(AS5600_I2C);
TwoWire I2Cone = TwoWire(0);
TwoWire I2Ctwo = TwoWire(1);

//Motor parameters
BLDCMotor motor = BLDCMotor(11);
BLDCDriver3PWM driver = BLDCDriver3PWM(32,33,25);

BLDCMotor motor1 = BLDCMotor(11);
BLDCDriver3PWM driver1 = BLDCDriver3PWM(26,27,14);

//Command settings
float target_velocity = 0;
Commander command = Commander(Serial);
void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); }
void doVelP(char* cmd) { command.scalar(&motor.PID_velocity.P, cmd); }
void doVelI(char* cmd) { command.scalar(&motor.PID_velocity.I, cmd); }
void doVelD(char* cmd) { command.scalar(&motor.PID_velocity.D, cmd); }
void doAngleP(char* cmd) { command.scalar(&motor.P_angle.P, cmd); }




void setup() {
  I2Cone.begin(19,18, 400000); 
  I2Ctwo.begin(23,5, 400000);
  sensor.init(&I2Cone);
  sensor1.init(&I2Ctwo);
  //Connect the motor object with the sensor object
  motor.linkSensor(&sensor);
  motor1.linkSensor(&sensor1);

  //Supply voltage setting [V]
  driver.pwm_frequency = 50000;
  driver.voltage_power_supply = 9;
  driver.init();

  driver1.voltage_power_supply = 9;
  driver1.init();
  //Connect the motor and driver objects
  motor.linkDriver(&driver);
  motor1.linkDriver(&driver1);
  
  //FOC model selection
  motor1.foc_modulation = FOCModulationType::SpaceVectorPWM;
  motor1.foc_modulation = FOCModulationType::SpaceVectorPWM;
  //Motion Control Mode Settings
  motor.controller = MotionControlType::angle;
  motor1.controller = MotionControlType::angle;



  //Speed PI loop setting
  motor.PID_velocity.P = 0.05;
  motor1.PID_velocity.P = 0.1;
  motor.PID_velocity.I = 0.3;
  motor1.PID_velocity.I = 1;

  //Angle P ring setting
  motor.P_angle.P = 1;
  motor1.P_angle.P = 20;

  //Speed low-pass filter time constant
  motor.LPF_velocity.Tf = 0.01;
  motor1.LPF_velocity.Tf = 0.01;


  
  //Max motor limit motor
  motor.voltage_limit = 3;
  motor.current_limit = 8;
  motor1.voltage_limit = 2;

  //Set a maximum speed limit
  motor.velocity_limit = 80;
  motor1.velocity_limit = 50;

  Serial.begin(9600);
  motor.useMonitoring(Serial);
  motor1.useMonitoring(Serial);

  
  //Initialize the motor
  motor.init();
  //motor1.init();
  //Initialize FOC
  motor.initFOC();
  //motor1.initFOC();
  command.add('T', doTarget, "target velocity");
  command.add('P', doVelP, "velocity P");
  command.add('I', doVelI, "velocity I");
  command.add('D', doVelD, "velocity D");
  command.add('A', doAngleP, "angle P");

  Serial.println(F("Motor ready."));
  Serial.println(F("Set the target velocity using serial terminal:"));
  
}

bool kill_switch = true; // true = enabled, false = disabled

void checkKillSwitch() {
  if (Serial.available()) {
    char c = Serial.read();
    if (c == '/') {
      kill_switch = !kill_switch;
      Serial.print("Kill switch: ");
      Serial.println(kill_switch ? "ON" : "OFF");
    }
  }
}

void loop() {
  checkKillSwitch();
  if (kill_switch) {
    return;
  }

  //Serial.print(sensor.getAngle()); 
  //Serial.print(" - "); 
  //Serial.print(sensor1.getAngle());
  //Serial.println();
  motor.loopFOC();
  //motor1.loopFOC();

  motor.move(target_velocity);
  //motor1.move(target_velocity);
  
  command.run();
}

In the videos below, you can see what is happening at different velocities.

2

The twitchy motion at the end of the first video looks like maybe the encoder got misaligned.

Printing to serial monitor can cause stuttering like in the second video. It seems to only happen if you make multiple calls to Serial.print in one frame, so you can work around it by first printing everything you want into a char array with sprintf and then make a single call to Serial.print. However the STM32 implementation of sprintf does not support floating point, so that’s a bit of a pain (I just multiply by 1000 and cast to int).

Another useful technique for debugging this kind of thing is to record data into an array every frame, and when you send a command via serial, stop the motor and print out the whole array. That way you can get a clear view of what it was thinking while behaving badly, without slowing it down with real-time printing.

3

I can’t help with this because I have no experience with this particular board. If we had one reasonably good board, I would have used it many times and would be able to help. As is, can’t help. This is what happens. We need to plan ahead and collaborate on one reasonably good board for many many reasons, and the body of experience that accumulates around it is a good reason.