Torque Mode to Angle Mode - motor working against target

applications
1

Goal: in a “dead zone” around the target angle the motor should be “off”*. High above and below torque mode is needed, so it´s active in that zone**.
But at a specific angle in that dead zone the motor should snap to a specific angle. For this, angle mode is activated shortly before it from both sides.

Problem: coming from higher angles towards the angle target it´s working (motor is forcing towards the target). Coming from lower angles the motor works → against ← (away from) the target.

The (essential) code:

#include <SimpleFOC.h>

float curr_pos = 0.00; // current position

// BLDC motor & driver instance
BLDCMotor motor = BLDCMotor(11);
BLDCDriver3PWM driver = BLDCDriver3PWM(5, 10, 6, 8);

// Sensor
MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C);


void setup() {

  // ## 1 Serial ##
  Serial.begin(115200);
  motor.useMonitoring(Serial);

  // ## 2 Sensor ##
  sensor.init();
  motor.linkSensor(&sensor);

  // ## 3 Driver ##
  driver.voltage_power_supply = 12; // power supply voltage [V]
  driver.init();
  motor.linkDriver(&driver);

  // ## 4 Control Types ##
  motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
  motor.controller = MotionControlType::torque;
  motor.phase_resistance = 12.6; // 12.6 Ohms
  motor.torque_controller = TorqueControlType::voltage;

  // ## 5 Controller Config ##
  // controller configuration based on the control type
  motor.PID_velocity.P = 0.2;
  motor.PID_velocity.I = 20;
  motor.PID_velocity.D = 0;
  
  // jerk control using voltage voltage ramp
  // default value is 300 volts per sec  ~ 0.3V per millisecond
  motor.PID_velocity.output_ramp = 1000;
  
  motor.voltage_limit = 4.00;

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

  // ANGLE mode setup
  // angle loop controller
  motor.P_angle.P = 20; 

  motor.velocity_limit = 20;

  // ## 6 Current Sense ##

  // ## 7 Initialise Motor ##
  motor.init();
  // align encoder and start FOC
  motor.initFOC();
  // set the inital target value
  motor.target = 2.0;

  Serial.println("Motor ready.");
  Serial.print("\n");

  _delay(1000);
}


void loop() {

  motor.loopFOC();
  motor.move();
  
  sensor.update();
  curr_pos = (sensor.getAngle());

  // no force ABOVE pos. 3.0
  if (curr_pos > 3.0) {
    motor.voltage_limit = 0;
    motor.target = 2.9;
    motor.controller = MotionControlType::torque;
  }

  // no force BELOW pos. 2.8
  if (curr_pos < 2.8) {
    motor.voltage_limit = 0;
    motor.target = 2.9;
    motor.controller = MotionControlType::torque;
  }

  // force towards pos. 2.9 when "near"
  if (curr_pos > 2.8 && curr_pos < 3.0) {
    motor.voltage_limit = 4;
    motor.target = 2.9;
    motor.controller = MotionControlType::angle;
  }

}

Setup: Nucleo stm32 G474RE, simpleFOC Shield 3.1, AS5600, BLDC iPower 5108.
I´m playing around with this for hours now. I´ve got a feeling it´s me being stupid again, but I can´t find the fault.

*enabling/disabling motor doesn´t work, it´s just buzzing after activation not reacting to anything.
**leaving it in Angle Mode there doesn´t change the issue. Having everything in Torque Mode is leading to the same result.

2

Some thoughts:

  • don’t call sensor.update() it’s called for you in loopFOC
  • reset the velocity PID when you enter angle mode (but not each time through the loop)
  • add a motor.motion_downsample or your move loop will probably be a bit too fast ok this MCU…
3

thanks for your thoughts and input! Downsampling is nice, I didn´t use it before. Having it on 2 seems to smoothen everything out a bit. Even with a rate of 10 I can´t see a difference in essential behaviour.

Unfortunately, it didn´t help, at least directly. But it made me deal with the issue a bit more and it brought things further:
widening the window for the target (2.9 in the example above, window now 3.3 > T > 2.5) showed that the force is always leading out to the lower side, no matter what direction the window is approached from.
I then set the target to -2.9.
Now it snaps to 2.9 and immediately begins oscillating itself out of the stated borders…

I started the Angle Control example script (w/ same PIDs/filters/etc.) and there the same thing happens: Target “1” → it moves to -1. Target “-2” → it moves to 2. Signs are switched. My serial output is straight Serial.print(sensor.getAngle());
The difference is, in the example script it stays put at the target, no wiggling whatsoever.
Is this happening a thing that was issued before somewhere? Those inconsitencies are a tough nut to crack for a “beginner”.

4

sensor.getAngle() is the “raw” angle. motor.shaft_angle is what the PID sees, and should be more useful for your debugging in this case. Presumably motor.sensor_direction is -1, so the multiply in FOCMotor::shaftAngle() will invert the direction from sensor.getAngle().

As runger says, you need to reset the velocity PID when entering angle mode, incase it has any lingering integral term from last time it was active.

5

thanks, that helped for understanding. Resetting didn´t help either. I guess I have to change my way of approach.

6

I don’t think there’s anything wrong with your approach. You just need to find out exactly where it’s going wrong. Thankfully SimpleFOC is indeed fairly simple, so there’s not a whole lot of code to dig through. Read through the BLDCMotor functions move and loopFOC, and pid.cpp.

I’d start by adding some Serial.print calls in BLDCMotor::move to catch when it’s applying torque in the opposite direction from the target angle, and then more in the PID update to figure out why.

7

Had to get my head free… aaand it works! Still rough, but now its “only” tuning. Thanks to you for pointing out some directions to look into!

It is solved mainly by giving the commands only once. Also, I stay in Angle Mode and motor enable/disable does the job, too. And I chose to work with the shaft angle only, what makes it easier for my 2.3 brain cells.

now looking something like

  // force towards pos. 2.9 when "near"
  if (curr_pos > 2.8 && CHECK == 0) {
    motor.enable();
    motor.target = 2.9;
    CHECK = 1;
  }

  // no force below 2.8
  if (curr_pos < 2.8 && CHECK == 1) {
    motor.disable();
    CHECK = 0;
  }

I recognized that setting motor.voltage_limit and/or motor.velocity_limit to 0 doesn´t have any effect whatsoever in my case. If that is correct behaviour then it´s somehow misleading. If the system is told “no voltage for you” it should go limp in my understanding. But I can adapt to it.