Problems getting roboter arm joint into closed loop angle control

hardware support
1

Hi, so for quite some time now I have been trying to get my roboter axis into closed loop control using the simple foc library, but I have stumbled into quite some issues :melting_face:

Here you can see my gimbal bldc motor (GM7008H) in open loop control:

Note that when I enter the target velocity when I run the code the motor starts oscilating and not moving at the desired speed. I have to also put the current limit down using the command prompt, which I find is pretty weird :upside_down_face:

The AS5047p magnetic encoder, which I read out via SPI works great and I already tested it seperately using the code from the github examples.

So here is the code I use for the angle control loop:

#include <SimpleFOC.h>

// magnetic sensor instance - SPI
MagneticSensorSPI sensor = MagneticSensorSPI(34, 14, 0x3FFF);
// magnetic sensor instance - MagneticSensorI2C
//MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C);
// magnetic sensor instance - analog output
// MagneticSensorAnalog sensor = MagneticSensorAnalog(A1, 14, 1020);

// BLDC motor & driver instance
BLDCMotor motor = BLDCMotor(11, 10.8, 28);
BLDCDriver3PWM driver = BLDCDriver3PWM(1, 2, 3, 4);
// Stepper motor & driver instance
//StepperMotor motor = StepperMotor(50);
//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6,  8);

// 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() {

  // initialise magnetic sensor hardware
  sensor.init();
  // link the motor to the sensor
  motor.linkSensor(&sensor);

  // driver config
  // power supply voltage [V]
  driver.voltage_power_supply = 24;
  driver.init();
  // link the motor and the driver
  motor.linkDriver(&driver);

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

  // 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 = 20;
  motor.PID_velocity.D = 0;
  // maximal voltage to be set to the motor
  motor.voltage_limit = 20;
  motor.current_limit = 1;

  // velocity low pass filtering time constant
  // the lower the less filtered
  motor.LPF_velocity.Tf = 0.01f;

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

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


  // initialize motor
  motor.init();
  // align sensor and start FOC
  motor.initFOC();

  // 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();
}

The bldc motor inits no problem, but then it doesn’t move to the desired position… If I send a target angle it does nothing. But what is interesting if I help with the hand it will move to a position and then hold this position.

Thanks for the help in advance…

2

Did you get simpleFOC working in closed loop with this motor? I am considering ordering it for my next project.

3

You need to tune the PID, and possibly kv as well.

For kv tuning, use torque mode. Target is in Amps, so should be less than current limit. The motor should stop moving with target 0, and accelerate up to full speed with a fairly small target, and accelerate up to full speed more quickly with larger target.

For PID tuning, use velocity mode. Keep D set to 0, it doesn’t work right. Usually I needs to be at least 10x higher than P, but the exact values vary a lot. Set up Commander or some other way to change them quickly so you can play with it.

Once that works, use angle mode and adjust angle P if needed. Usually the default 20 works, but sometimes lower values down to around 10 can give smoother start/stop. Sometimes you have to adjust velocity PI values a little more too.