Help with FOC of NEMA 17 Stepper Motor with as5600, L298N, and Arduino Mega 2560

hardware support
, , ,
1

Looking for support for FOC control of a NEMA 17 Stepper Motor, with as5600 magnetic sensor, L298N motor driver, and Arduino Mega 2560 MCU.

Open loop control works, (but noisy), but when I switch over to closed loop, the motor jitters.

I have been working in FOC Studio to tune the PID values, but with no luck. I have ran the magnetic encoder standalone example code and have received great values from the sensor when manually turning the motor shaft. What am I missing?

See the code below:

#include <SimpleFOC.h>


StepperMotor motor = StepperMotor(50);
StepperDriver4PWM driver = StepperDriver4PWM(5,6,9,10);

// Example of AS5600 configuration 
MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C);


// commander interface
Commander command = Commander(Serial);
void doMotor(char* cmd){ command.motor(&motor, cmd); }

void setup() {

  // initialize encoder sensor hardware
  sensor.init();
  
  // link the motor to the sensor
  motor.linkSensor(&sensor);

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

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

  // set control loop type to be used
  motor.controller = MotionControlType::torque;

  // controller configuration based on the control type 
  motor.PID_velocity.P = 0;
  motor.PID_velocity.I = 0;
  motor.PID_velocity.D = 0;
  // default voltage_power_supply
  motor.voltage_limit = 24;

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

  // angle loop controller
  motor.P_angle.P = 0;
  // angle loop velocity limit
  motor.velocity_limit = 50;

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

  // initialise motor
  motor.init();
  // align encoder and start FOC
  motor.initFOC();

  // set the initial target value
  motor.target = 2;

  // define the motor id
  command.add('M', doMotor, "motor");

  // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com)
  Serial.println(F("Motor commands sketch | Initial motion control > torque/voltage : target 2V."));
  
  _delay(1000);
}


void loop() {
  // iterative setting FOC phase voltage
  motor.loopFOC();

  motor.monitor();
  // iterative function setting the outter loop target
  // velocity, position or voltage
  // if tatget not set in parameter uses motor.target variable
  motor.move();

  // user communication
  command.run();
}
2

Hey @Clayton_Young,

Noisy open loop is probably because of the very high motor.voltage_limit
It will create a very high current in the motor and make all kinds of noises :smiley:
Try reducing the voltage limit to 1 or 2 volts. The current induced in the motor will be proportional to this voltage (at least in the open-loop mode)

I = \frac{Voltage}{Resistance} 
motor.voltage_limit = 1; // Volt

Now in terms of jittery closed loop, could you copy here your monitoring output of the motor initialization (the text that is printed in your serial terminal). This might help us to find the source of the problem.

In the torque control mode using voltage command, no PID is being used so no changes in the PID values will make a difference. You can see this better in the docs, the block scheme does not have any PIDs for voltage torque control.

3

Thank you for the reply. I turned down the voltage limit to 4 volts. See the updated code I am using below.

And here is the motor monitoring output:

Connected …
MOT: Init
MOT: Enable driver.
MOT: Align sensor.
MOT: sensor_direction==CCW
MOT: PP check: OK!
MOT: Zero elec. angle: 4.94
MOT: Ready.

I have been messing with the PID values in velocity closed loop control with no luck. As I turn up the “P” value the motor will start to jitter, but never rotates more than 15 degrees (you can hear it jitter in the video).

Here are two attached videos. The first showing open loop velocity and then a switch to closed loop using the FOC Studio software. The second shows the motor jittering.

What am I missing?

#include <SimpleFOC.h>


StepperMotor motor = StepperMotor(50);
StepperDriver4PWM driver = StepperDriver4PWM(9,10,5,6);

// Example of AS5600 configuration 
MagneticSensorI2C encoder = MagneticSensorI2C(AS5600_I2C);

// commander interface
Commander command = Commander(Serial);
void doMotor(char* cmd){ command.motor(&motor, cmd); }

void setup() {

  // initialize encoder sensor hardware
  encoder.init();
  // link the motor to the sensor
  motor.linkSensor(&encoder);

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

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

  // set control loop type to be used
  motor.controller = MotionControlType::velocity;

  // controller configuration based on the control type 
  motor.PID_velocity.P = 0;
  motor.PID_velocity.I = 0;
  motor.PID_velocity.D = 0;
  // default voltage_power_supply
  motor.voltage_limit = 4;
  motor.current_limit = 2;
  motor.sensor_direction = CCW;

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

  // angle loop controller
  // motor.P_angle.P = 20;
  // angle loop velocity limit
  motor.velocity_limit = 50;

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

  // initialise motor
  motor.init();
  // align encoder and start FOC
  motor.initFOC();

  // set the initial target value
  motor.target = 5;

  // define the motor id
  command.add('M', doMotor, "motor");

  motor.useMonitoring(Serial);

  // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com)
  Serial.println(F("Motor commands sketch | Initial motion control > torque/voltage : target 2V."));
  
   _delay(1000);
}


void loop() {
  // iterative setting FOC phase voltage
  motor.loopFOC();

  motor.move();

  motor.monitor();
  // user communication
  command.run();
}
4

What happens if you set the speed way down, like way way down? Also I would loop on motor.loopFOC() like 50 times before doing the other stuff.

I found dealing with things very hard, picking things apart is quite hard. You can use the commander that Deku used to check the various variables etc. Use serial monitor and serial plotter to look in there and follow the flow of how you were thinking things were going, see if you can spot where things have started to go wrong.

Here is the code you can use to check various variables etc, add the lines and variables you need:

void SerialComm()
{
  switch(Serial.read())
  {
  case 'T': goal_speed = Serial.parseFloat(); Serial.print("T");break;
  case 't': Serial.print("T:"); Serial.println(goal_speed); break;

  case 'V': v_diff = Serial.parseFloat(); Serial.print("V");break;
  case 'v': Serial.print("V:"); Serial.println(v_diff); break;
  
 // case 'P': p_gain = Serial.parseFloat(); Serial.print("P");break;
 // case 'p': Serial.print("p:"); Serial.println(p_gain); break;
 // case 'I': diff_filter.Tf = Serial.parseFloat(); Serial.print("I");break;
//  case 'i': Serial.print("f:"); Serial.println(diff_filter.Tf); break;
 // case 'D': d_gain = Serial.parseFloat(); Serial.print("d_gain set");
 // case 'd': Serial.print("d_gain is:"); Serial.println(d_gain); break;
//  case 'O': i_windup_limit = Serial.parseFloat(); Serial.print("i_windup_limit set");
  //case 'o': Serial.print("windup limit is:"); Serial.println(i_windup_limit); break;
//  case 'U': setpoint = Serial.parseFloat(); Serial.print("S"); break;
//  case 'u': Serial.print("s:"); Serial.println(setpoint); break;
  
  }
}

If you are planning to do anything more than extremely basic, my plan should I ever have to do anything with Arduino again was to learn to use Atmel studio and their debugging tools. It’s supposed to be arduino compatible but you can do actual debugging, look into the memory of the device and see what’s happening at high speed. Print statements don’t work well to spot things that are happenign fast.

I also used functions like this a lot, add the stuff you want to print and then use the serial plotter to spot patterns etc.

void print_pid_stuff(){

  //Serial.print("rawcount:");
  //Serial.print(sensor.getRawCount()); 
  //Serial.print(",");
  Serial.print("calsens:");
  Serial.print(sensor_calibrated.getMechanicalAngle()); 
  Serial.print(",");
  Serial.print("e:");
  Serial.print(e); 
  Serial.print(",");
  Serial.print("s:");
  Serial.print(s); 
  Serial.print(",");
  Serial.print("cs:");
  Serial.print(control_signal); 
  Serial.print(",");
  //Serial.print("csa:");
  //Serial.print((sensor_calibrated.getMechanicalAngle())); 
  //Serial.print(",");
  Serial.print("apd:");
  Serial.println(average); 
  
}

Also I noticed you are setting the current, which I think will have no effect in this situation, I don’t know if it may be causing a rpoblem, I would think not.

1 Like
5

Hey,

Looking at your code, of course it won’t move with the PID values at 0. So you set P and I via SimpleFOCStudio, I assume - I couldn’t 100% follow in the video what’s going on.

One thing is the motor vs. sensor, this can be hard to tune because the AS5600 is not that precise, while a 50 PP motor has very many electrical revolutions per physical (=higher frequency waveforms). With its high latency due to I2C the AS5600 isn’t a good choice for this motor.

So for sure do the initial tests at low speeds. You may also consider setting the output_ramp of the PID to a low value to prevent the motor changing speeds too quickly.

Then you’ll have to tune the PID, which could be quite a difficult/fussy task given the motor/sensor. Set a speed like 1 rad/s, and then slowly raise P until you get movement and it begins to try to track the target value. Increase it more until you get oscillations, then back it down again. Then raise I until it tracks the target value well.

Another question: is the torque-voltage mode working for you well? This is the first mode to try, you don’t need to tune a PID for this…

1 Like
6

Would you recommend the 14-bit version, like the AS5048A?

7

Yes, I think with this type of sensor which is SPI (much faster) and more precise I think it will be easier for you…

8

Hello everyone.
Its been a time since the last reply to this post but i hope i can help you with somethings @Clayton_Young, so…
I was checking the code and documentation about the implementation of FOC algorithms for a stepper motors class and i’ve notice you’re changing the motor.foc_modulation and you’re select SpaceVectorPWM type… well according to the current library version and documentation, for the stepper motor class this modulation type have no support (link for doc) i’ve verify that in the library source code , and for me, part of the problem is this code line.
Another part of the problems you have with the motor control are the problems mentioned by @runger related to the resolution and communication protocol of the AS5600 encoder and the PID output_ramp when you’ll use the position/velocity control mode. My recommendation is you’ll have change the encoder and the modulation type code line for: motor.foc_modulation = FOCModulationType::SinePWM;

p.d.: I’m not a native english speaker so, i’ll appreciate your help and understanding with that, and also is my first post haha.

1 Like
9

Did you managed to solve it? I have similar setup with NEMA17 stepper motor with L298n. pin in1-in4 connected to Arduino MEGA pin 8-11. AS5600 magnetic sensor over i2c. Direction pin is directly connected to ground.
pin 22 and 23 is VCC and GND for the sensor (as I ran out of 5v pins in the MEGA(2 used for pullup resistor for i2c bus.)

/**
 *
 * Position/angle motion control example
 * Steps:
 * 1) Configure the motor and magnetic sensor
 * 2) Run the code
 * 3) Set the target angle (in radians) from serial terminal
 *
 */
#include <SimpleFOC.h>

// magnetic sensor instance - SPI
// MagneticSensorSPI sensor = MagneticSensorSPI(AS5147_SPI, 10);
// 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);
// BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8);
// Stepper motor & driver instance
StepperMotor motor = StepperMotor(50);
StepperDriver4PWM driver = StepperDriver4PWM(8, 9, 10, 11);

// 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 doMotor(char* cmd) {
  command.motor(&motor, cmd);
}
void sensor_output() {
  sensor.update();
  Serial.print("sensor.getSensorAngle:");
  Serial.println(sensor.getSensorAngle());
}

void setup() {
  pinMode(22, OUTPUT);
  pinMode(23, OUTPUT);
  digitalWrite(22, HIGH);
  digitalWrite(23, LOW);

  // 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 = 12;
  driver.pwm_frequency = 32000;
  driver.init();
  // link the motor and the driver
  motor.linkDriver(&driver);

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

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

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

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

  // use monitoring with serial
  Serial.begin(115200);
  // 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");
  command.add('M', doMotor, "motor");

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


void loop() {
  // sensor_output();
  // 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);
  // motor.monitor();

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

  // user communication
  command.run();
}

This is copied from libraries examples and modified. pins 22 and 23 are used for AS5600 VCC and GND( I ran out of 5v pins as 2 pullup resistors are attached to i2c bus).
Output goes like this.

17:34:05.273 -> MOT: Monitor enabled!
17:34:05.273 -> MOT: Init
17:34:05.755 -> MOT: Enable driver.
17:34:06.752 -> MOT: Align sensor.
17:34:09.967 -> MOT: sensor_direction==CCW
17:34:09.968 -> MOT: PP check: fail - estimated pp: 58.51
17:34:10.709 -> MOT: Zero elec. angle: 4.51
17:34:10.902 -> MOT: Ready.
17:34:10.902 -> Motor ready.
17:34:10.902 -> Set the target angle using serial terminal:

At this point the motor only oscillates. If I set for example T1 it tries to go in the direction but not the whole way. again if I set T-1 it goes in the other direction but not the whole angular distance(as prev target was 1rad it should now come back to the position it things is 0 and then go 1rad in the other direction, right? But it doesn’t even reach the initial zero position).
I have tried with larger velocities. No predictable behavior. Some times it doesn’t do anything.

For verification of connection. The motor works fine in openloop mode and the sensor.getSensorAngle() returns the angle if printed to serial

10

IS your code really using driver frequency 32kHz? This is way too fast for L298N, you need to set it slower. This driver is very old and the performance is pretty bad, and the same can be said for AS5600, but it should work a little bit. Try setting PWM frequency to 4-5kHz.

11

Sorry my bad. I have tried with 4000 too. it still doesn’t work(same behavior)