Closed Loop Velocity Control with ESP 32

Hello

I am trying to run a motor in Closed-loop velocity control this is my setup

ESP32
DRV8302 driver board
Maxon EC I 30 motor using Hall sensors as feedback

I have had the motor run in Open-loop and Closed-loop control voltage with no problem

My problem is after the motor has run through its initial alignment and calibration routine (without any problems) the motor just goes mad for a second then stops.
The Volt_Q returns nan and Vel return inf in the monitor.

I have try turning the P term down very low wit the I and D terms set to 0 but this has no affect

What am I doing wrong?

here is my code


#include <SimpleFOC.h>

//define Pinout DRV8302
#define INH_A 25 //PWM_A
#define INH_B 26 //PWM_B
#define INH_C 27 //PWM_C

#define EN_GATE 16
#define M_PWM 17 
#define M_OC 18
#define OC_ADJ 19

// Postion Sensor Hall's
#define HallA 21
#define HallB 22 
#define HallC 23
#define PP 4      //Pole Pairs




// BLDC motor & driver instance
// BLDCMotor motor = BLDCMotor(pole pair number, phase resistance (optional) );
BLDCMotor motor = BLDCMotor(PP);
// BLDC driver instance
BLDCDriver3PWM driver = BLDCDriver3PWM(INH_A, INH_B, INH_C, EN_GATE);

// Hall sensor instance
HallSensor sensor = HallSensor(HallA, HallB, HallC, PP);

// Interrupt routine intialisation
// channel A and B callbacks
void doA(){sensor.handleA();}
void doB(){sensor.handleB();}
void doC(){sensor.handleC();}


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

/////////////////////////////////////////////////////////////////////////////////////////////////

void setup() {

  // DRV8302 specific code
  // M_OC  - enable overcurrent protection
  pinMode(M_OC,OUTPUT);
  digitalWrite(M_OC,LOW);
  // M_PWM  - enable 3pwm mode
  pinMode(M_PWM,OUTPUT);
  digitalWrite(M_PWM,HIGH);
  // OC_ADJ - set the maximum overcurrent limit possible
  // Better option would be to use voltage divisor to set exact value
  pinMode(OC_ADJ,OUTPUT);
  digitalWrite(OC_ADJ,HIGH);

  // initialize sensor hardware
  sensor.pullup = Pullup::USE_EXTERN;
  sensor.init();
  sensor.enableInterrupts(doA, doB, doC); 
  // link the motor to the sensor
  motor.linkSensor(&sensor);

  // pwm frequency to be used [Hz]
  // esp32 configurable
  driver.pwm_frequency = 50000;
  // power supply voltage [V]
  driver.voltage_power_supply = 12;
  driver.init();
  // link the motor and the driver
  motor.linkDriver(&driver);

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

  // set motion control loop to be used
  motor.torque_controller = TorqueControlType::voltage;
  motor.controller = MotionControlType::velocity;

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

  // velocity PI controller parameters
  motor.PID_velocity.P = 1.0f;
  motor.PID_velocity.I = 0;
  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;

  // Setup for monitor
  Serial.begin(115200); // the higher the better
  motor.useMonitoring(Serial);
  //display variables
  motor.monitor_variables =  _MON_TARGET | _MON_VOLT_Q | _MON_VEL; // set monitoring of d and q currents 
  // downsampling
  motor.monitor_downsample = 100; // default 10


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

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

  Serial.println(F("Motor ready."));
  Serial.println(F("Set the target velocity 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_velocity);

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

  // user communication
  command.run();
}

Hi,

Start with PID values of Zero and using the commander interface start changing their values. I also had a similar issue and found out that the Hall sensor required a pull-up resistor after adding the pull-up resistor, I was able to tune the PID parameter.

I used a sensor align voltage of 0.7.

Hi MoidKhan,

Yes, I have pullups on the hall effects, it would not return a motor position data without them.

If I turn all the PID values to Zero this will effectively turn the control off.

Yes, It will, but you can turn the motor on again by changing the P value first. when you see that the motor is moving fine with P-value, start changing the I value and then at the end D value.

That’s my problem, even if I have a really low value (say 0.01) for the P term and I and D set to 0. the motor just goes mad totally random action. The loop seems to crash returning the voltage value as ‘Not a Number’ and the velocity as infinite.

Well, your code and everything seem to be fine. Try it by reducing the output ramp value. Make it a low and then check how your motor responds.
Thanks

Hey @Rollmop,

Does your torque control loop work?
If you set

 motor.controller = MotionControlType::torque;

There you should be able to control the motor torque, roughly the higher voltage set point the higher verlocity, negative voltage inverse direction.

hi @Antun_Skuric ,

Yes, it ran ok in closed-loop Torque mode.

If I change this code MotionControltypr to torque it operates the same way if the target is position it rotates clockwise if the target is negative counterclockwise.

Do you see the messages during motor initialisation? Could you post a copy of these? If you’re not seeing them a while (!Serial) ; after Serial.begin(115200); can help.

Hi @runger ,

Here are the messages from the intialisation.

09:02:28.327 -> MOT: Monitor enabled!
09:02:28.327 -> MOT: Init
09:02:28.847 -> MOT: Enable driver.
09:02:29.833 -> MOT: Align sensor.
09:02:32.035 -> MOT: sensor_direction==CW
09:02:32.035 -> MOT: PP check: OK!
09:02:32.784 -> MOT: Zero elec. angle: 3.14
09:02:33.485 -> MOT: No current sense.
09:02:33.485 -> MOT: Ready.
09:02:33.485 -> Motor ready.
09:02:33.485 -> Set the target velocity using serial terminal:

This is how the motor runs before it crashes
Target: Black, Voltage: Green, Velocity:Red

My controller settings are:

// set motion control loop to be used
  motor.torque_controller = TorqueControlType::voltage;
  motor.controller = MotionControlType::velocity;

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

  // velocity PI controller parameters
  motor.PID_velocity.P = 0.1f;
  motor.PID_velocity.I = 0;
  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 = 300;

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

Hey @Rollmop,

Did you try the sensor alone?
Using one of the hall sensor stand alone examples?
Can you plot the velocity and the angle value and see if you have the same behavior?

Hello @Antun_Skuric

Yes, I have been through all the stages testing the sensor and the driver, also running it in an open loop.
I did initially have a problem with the hall sensor signal (see my other post) but I think this is fixed and I am getting good position data.

Here is a plot of my setup running in torque mode, with the target set to 0.25 (it’s


a high-performance motor so can run at fairly high speeds at low voltages)
Voltage: Green, Velocity: Red, Angle: Blue

Small update, I tried going back to an Arduino Mega to see if the problem was with the ESP32. The result was it didn’t even get through the initialisation process the motor just locked in one position during the alignment process

Hey @Rollmop,

This is very strange. if the motor locks during the init that most probably has nothing to do with the sensor.

Your velocity measurement is very noisy, but it seems ok in the second plot. It is possible that we have an issue witht the HallSensor class somewhere. We did not tuch it for some time now.
However, it was developed on esp32 by @Owen_Williams so it definitely should work both for arduinos and esp32s.

I’ll try to test it tonight and I’ll let you know if I can reproduce it.

The first plot is when I try to run in Velocity Closed Loop (it does not work) the second plot is running in Torque Closed loop

Hey @Rollmop,

I understand. How does it look like when you use velocity control. The motor starts spinning in one direction and then it stops.

Was that the behavior when you took this screenshot?
Did your motor start and stop like it seems like from the velocity profile?

Did you try to use the very very low values of velocity P gain, under 0.01?
You can fix I and D to zero for now.
you can also try using the ramp value, try settting the PID_velocity.ramp=1000
You can also maybe test with a lower value under 200.

You can also try downsampling the motion control loop by using the motor.motion_downsample parameter. Try setting the downsampling between 10 and 100 .

hi @Antun_Skuric

Lots of questions there I’ll try and answer them without disappearing into a rabbit hole.

Yes, this plot is when I try to run the motor in closed-loop velocity mode using hall sensors as feedback. The motor does turn very erratically (as you can see from the velocity profile) until the loop seems to crash. Voltage returns ‘nan’ and velocity returns ‘inf’

I have turned the P term as low as 0.02 with I and D terms set to 0. I can get the motor to run if the target is above 10 rad/s although the motion is very lumpy. If I set the target below this or turn the P term any lower the motor will not run. If I go higher with the P term I just get the erratic behavior.
I have tried changing the motor.PID_velocity.output_ramp to both high and low values (10 to 1000) but this does seem to have any effect.

For the monitoring, I am using motor.monitor_downsample does motor.motion_downsample offer any different functionality?

Hello
can you try to disable velocity filter ?
// velocity low pass filtering time constant
motor.LPF_velocity.Tf = 1.0f; //0.01f;

@Rollmop This problem has been solved? Because i got the same problem.