Position Control: Too much current absorption at initFOC()

Hi,

I’m testing the SimpleFOC library for the first time on a B-G431B-ESC1 and a BLDC motor like the flipsky 5065 270KV. I am currently using Arduino IDE with STM32duino core.

After some tests in which at startup my power supply went into OCP and consequently the sensor alignment failed (verified with motor.monitor()), by disabling the OCP of the power supply the sensor alignment procedure seems to be going successfully and the motor move smoothly (But the power supply voltage drop to 10 V due to the current limit). Again using the motor.monitor() seems that the position reference coming from the hall sensors is fine and precise.

However after this procedure, by trying to rotate manually the rotor, the position control work but in a strange fashion. In the sense that there are positions in which the motor stick the position and with a little manual help, start to jerky rotate till to the target position. The strange things is that this behavior seems to be accentuated in the clockwise direction.

The setup was tested before with the ST motor pilot (it works good) and motor profiler, and such motor parameters are reported on the arduino sketch.

One things that i’ve tried to tune (for now i haven’t touch the PID) is the current_limit (that seems to impact only on the motor.move() and not in the motor.initFOC()) and provide an zero_electric_offset to motor.initFOC() without solve the problem (in the ST motor control was 60°).

Right now i’ve saw that voltage_sensor_align is used in the alignSensor() and so by tuning this parameter i should avoid the OCP, but i’ve not tested yet.

So to conclude, I wanted to ask you some advice on how to calibrate the control correctly, maybe I should start with something as simple as speed control?

i leave the code of the sketch

/** 
 * B-G431B-ESC1 position motion control example with encoder
 *
 */
#include <SimpleFOC.h>
// software interrupt library

const int motorPolePairs = 9;

// Motor instance
BLDCMotor motor = BLDCMotor(motorPolePairs);
BLDCDriver6PWM driver = BLDCDriver6PWM(A_PHASE_UH, A_PHASE_UL, A_PHASE_VH, A_PHASE_VL, A_PHASE_WH, A_PHASE_WL);
LowsideCurrentSense currentSense = LowsideCurrentSense(0.003, -64.0/7.0, A_OP1_OUT, A_OP2_OUT, A_OP3_OUT);
//(resistance, gain, pin_1_2_3)

// encoder instance

HallSensor sensor = HallSensor(A_HALL1, A_HALL2, A_HALL3, motorPolePairs);

// Interrupt routine intialisation
// channel A and B callbacks
void doA(){sensor.handleA();}
void doB(){sensor.handleB();}
void doC(){sensor.handleC();}
// If no available hadware interrupt pins use the software interrupt

// angle set point variable
float target_angle = 0;
// instantiate the commander
Commander command = Commander(Serial);
void doTarget(char* cmd) { command.scalar(&target_angle, cmd); }

void setup() {

  Serial.println(F("Motor ready."));
  
  // initialize sensor hardware
  sensor.init();
  sensor.enableInterrupts(doA, doB, doC);
    
  // driver config
  // power supply voltage [V]
  driver.voltage_power_supply = 24;
  driver.init();
  
  motor.phase_resistance = 0.2; // [Ohm]
  motor.current_limit = 5;      // [Amps] - if phase resistance defined
  //motor.voltage_limit = 0.5; // [V] - if phase
                           //    resistance not defined
  motor.velocity_limit = 50; // [rad/s] 5 rad/s cca 50rpm

  // link the motor to the sensor
  motor.linkSensor(&sensor);
  // link the motor and the driver
  motor.linkDriver(&driver);


  // current sensing
  currentSense.init();
  // no need for aligning
  currentSense.skip_align = true;
  motor.linkCurrentSense(&currentSense);

  // aligning voltage [V]
  motor.voltage_sensor_align = 3;
  // index search velocity [rad/s]
  motor.velocity_index_search = 3;

  // set motion control loop to be used
  motor.controller = MotionControlType::angle;

  // contoller configuration 
  // default parameters in defaults.h

  // velocity PI controller parameters
  motor.PID_velocity.P = 0.2f;
  motor.PID_velocity.I = 2;
  motor.PID_velocity.D = 0;
  // default voltage_power_supply
  motor.voltage_limit = 6;
  // jerk control using voltage voltage ramp
  // default value is 300 volts per sec  ~ 0.3V per millisecond
  motor.PID_velocity.output_ramp = 1000;
 
  // velocity low pass filtering time constant
  motor.LPF_velocity.Tf = 0.01f;

  // angle P controller
  motor.P_angle.P = 20;
  //  maximal velocity of the position control
  motor.velocity_limit = 4;


  // use monitoring with serial 
  Serial.begin(115200);
  // comment out if not needed
  motor.useMonitoring(Serial);

  motor.monitor_variables = _MON_ANGLE; // default _MON_TARGET | _MON_VOLT_Q | _MON_VEL | _MON_ANGLE
    
  // Serial.println(F("Motor init."));

  // initialize motor
    
  Serial.println(F("Motor init."));
  motor.init();
  
  // align encoder and start FOC
  // Serial.println(F("Motor align."));

  motor.initFOC();
  Serial.println(F("Init FOC"));

  // add target command T
  command.add('T', doTarget, "target angle");

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

void loop() {

  // main FOC algorithm function
  // the faster you run this function the better
  // Arduino UNO loop  ~1kHz
  // Bluepill loop ~10kHz
  motor.loopFOC();
  
  // Motion control function
  // velocity, position or voltage (defined in motor.controller)
  // this function can be run at much lower frequency than loopFOC() function
  // You can also use motor.move() and set the motor.target in the code
  motor.move(target_angle);

  // function intended to be used with serial plotter to monitor motor variables
  // significantly slowing the execution down!!!!
  // motor.monitor();
  
  // user communication
  command.run();
}

Im quite sure those are 7 pole pairs. Not sure it´s a fix for your issue, but try that.

Looking forward to see how your test goes. Keep us posted.

Hi @luzz94 , welcome to SimpleFOC!

@Juan-Antonio_Soren_E is totally right, this motor has 14 poles, which means 7 pole pairs. Fixing this should significantly improve things.

In terms of high current draw at startup, try using the parameter:

motor.voltage_sensor_align = 0.5f

This will limit the voltage used during sensor alignment, and can prevent the high current draw. The 0.5 is just an example. You can pick a value where the motor moves well but the current isn’t too high.
This is a good idea for safety, in case you ever connect it to a battery (or stronger power supply), which doesn’t limit the current the way yours does now.

Thank you so much for your support!

Yesterday i’ve seen that there was a guide from @Owen_Williams on youtube that cover the most of my doubt on the tuning procedure, so thanks!

In fact by lowering the voltage_sensor_align now the current is no more so high and the alignment procedure seems ok.

@Juan-Antonio_Soren_E thank you! the motor is not branded, and from the ebay seller was reported 9 pole pairs, but by feeding two phases with an 1 A current supply and turning manually the motor, i’ve counted 7 pole pairs and therefore should be 14.

Anyway the motor still have that strange behavior (like a stepper motor that lose step) and so i’ll restart from a speed control loop and after tuned the controller i’ll come back to the angular control.

Before going into velocity control I suggest you make sure torque/ voltage mode is working as intended.

Thank you @David_Gonzalez for your suggestion!

yesterday in the lab i’ve tried the torque control mode (first voltage and then foc_current) but the motor still have a strange sound when it move.

i tried to start with i_d and i_q PID parameters all zero and tune first the P terms using the commander via serial interface, but with no success.

The strange thing that lead me to think that the problem is not coming from the ctr is that in the alignment phase the motor move smoothly and without any sounds, this problem is visible in this video.

What’s bothering me is that from the serial feedback of the alignment phase the PP check fail for any motor pole pair that i put (i.e. with 7 give 8.4 with 9 give 10.4 or so). and the zero elec. angle is changed when i’ve coupled the motor with the gearbox (before 1.04 and now 2.09).

Maybe one reason that the PP check fail is that the motor seems to have an unbalance in the phases, due to the fact that spin more slowly when i spin the motor in the counter clockwise instead of clockwise.

i’m pretty sure that is my fault, due to the fact that i’ve replaced the connectors without knowing that the copper wires was glazed. Obviously i’ve removed the glaze but maybe not entirely, creating a bad soldering connections and so this unbalance…

Anyway, with the ST motor control the motor move quite well, so maybe the functioning is not compromised but it affect the alignment phase. Unfortunately there’s no the possibility to have an oscilloscope in the lab, to verify the unbalance from the phases by back emf…

So, if there is someone that can suggest me something or help is very appreciated!

I wouldn’t try to use foc_current until voltage mode is working perfectly.

This can be cause also by a misalignment with the sensor and the electric angle. You could try to increase a little of the voltage limit during sensor alignment and see if PP check succeeds that way. Is the Zero elec. angle similar each time or is it somewhat different?