SimpleFOClibrary support for stepper motors

Winding up sound is the problem of the pwm frequency. 20kHz is well in the sound level. You should put at least 25-30khz in order to remove the sound.

I am not sure what is going on really. I’ll have to investigate it a bit more in depth.
I did not have such problem yet.

Could you show us a short vido of shats going on. Or/and add the motor.monitor() to the loop() and show us the terminal outputs.

13:17:02.865 -> MOT: Monitor enabled!
13:17:02.865 -> MOT: Init pins.
13:17:02.865 -> MOT: PWM config.
13:17:03.381 -> MOT: Enable.
13:17:04.348 -> MOT: Align sensor.
13:17:06.745 -> MOT: natural_direction==CCW
13:17:09.407 -> MOT: Absolute zero align.
13:17:09.919 -> MOT: Not available!
13:17:10.397 -> MOT: Motor ready.
13:17:10.397 -> Motor ready.
13:17:10.397 -> Set the target voltage using serial terminal:
13:17:11.413 -> 6.00	0.00	0.00
13:17:11.413 -> 6.00	0.00	0.00
13:17:11.413 -> 6.00	0.00	0.00
13:17:11.413 -> 6.00	0.00	0.00
13:17:11.413 -> 6.00	0.00	0.00
13:17:11.413 -> 6.00	0.00	0.00
13:17:11.413 -> 6.00	0.00	0.00
13:17:11.413 -> 6.00	0.00	0.00
13:17:11.413 -> 6.00	0.00	0.00
13:17:11.413 -> 6.00	0.00	0.00
13:17:11.413 -> 6.00	0.00	-0.13
13:17:11.413 -> 6.00	0.00	0.01
13:17:11.413 -> 6.00	0.00	0.12
13:17:11.413 -> 6.00	0.00	0.20
13:17:11.413 -> 6.00	0.00	0.26
13:17:11.413 -> 6.00	0.00	0.07
13:17:11.413 -> 6.00	0.00	-0.08
13:17:11.413 -> 6.00	0.00	0.06
13:17:11.413 -> 6.00	0.00	0.16
13:17:11.413 -> 6.00	0.00	0.24
13:17:11.413 -> 6.00	0.00	0.07
13:17:11.446 -> 6.00	0.00	0.17
13:17:11.446 -> 6.00	0.00	0.26
13:17:11.446 -> 6.00	0.00	0.32
13:17:11.446 -> 6.00	0.00	0.38
13:17:11.446 -> 6.00	0.00	0.17
13:17:11.446 -> 6.00	0.00	0.01
13:17:11.446 -> 6.00	0.00	0.13
13:17:11.446 -> 6.00	0.00	0.22
13:17:11.446 -> 6.00	0.00	0.29
13:17:11.446 -> 6.00	0.00	0.35

This is with torque voltage of 6

I don’t think you will see anything else with a video. Let me try velocity control as well.

13:21:11.368 -> MOT: Monitor enabled!
13:21:11.368 -> MOT: Init pins.
13:21:11.368 -> MOT: PWM config.
13:21:11.850 -> MOT: Enable.
13:21:12.841 -> MOT: Align sensor.
13:21:15.231 -> MOT: natural_direction==CCW
13:21:17.903 -> MOT: Absolute zero align.
13:21:18.382 -> MOT: Not available!
13:21:18.893 -> MOT: Motor ready.
13:21:18.893 -> Motor ready.
13:21:18.893 -> Set the target velocity using serial terminal:
13:21:19.880 -> -0.00	0.00	0.07
13:21:19.880 -> -0.01	0.00	0.00
13:21:19.880 -> -0.00	0.00	-0.07
13:21:19.880 -> 0.01	0.00	0.03
13:21:19.880 -> -0.01	0.00	-0.04
13:21:19.880 -> 0.01	0.00	0.03
13:21:19.880 -> -0.01	0.00	0.09
13:21:19.880 -> -0.02	0.00	0.15
13:21:19.880 -> -0.04	0.00	0.20
13:21:19.880 -> -0.06	0.00	0.24
13:21:19.914 -> -0.07	0.00	0.14
13:21:19.914 -> -0.06	0.00	0.19
13:21:19.914 -> -0.07	0.00	0.10
13:21:19.914 -> -0.05	0.00	0.01
13:21:19.914 -> -0.04	0.00	0.08
13:21:19.914 -> -0.05	0.00	0.14
13:21:19.914 -> -0.07	0.00	0.19
13:21:19.914 -> -0.09	0.00	0.24
13:21:19.914 -> -0.11	0.00	0.28
13:21:19.914 -> -0.12	0.00	0.18
13:21:19.914 -> -0.10	0.00	0.09
13:21:19.914 -> -0.09	0.00	0.15
13:21:19.914 -> -0.11	0.00	0.19
13:21:19.914 -> -0.12	0.00	0.24
13:21:19.914 -> -0.14	0.00	0.27
13:21:19.914 -> -0.15	0.00	0.17
13:21:19.914 -> -0.14	0.00	0.22
13:21:19.914 -> -0.16	0.00	0.27
13:21:19.914 -> -0.17	0.00	0.30
13:21:19.914 -> -0.19	0.00	0.34
13:21:19.914 -> -0.21	0.00	0.36
13:21:19.914 -> -0.22	0.00	0.24
13:21:19.914 -> -0.21	0.00	0.28
13:21:19.949 -> -0.22	0.00	0.31
13:21:19.949 -> -0.24	0.00	0.34
13:21:19.949 -> -0.25	0.00	0.22
13:21:19.949 -> -0.23	0.00	0.12
13:21:19.949 -> -0.22	0.00	0.17
13:21:19.949 -> -0.23	0.00	0.22
13:21:19.949 -> -0.25	0.00	0.26
13:21:19.949 -> -0.27	0.00	0.29
13:21:19.949 -> -0.28	0.00	0.32
13:21:19.949 -> -0.30	0.00	0.21
13:21:19.949 -> -0.28	0.00	0.12
13:21:19.949 -> -0.27	0.00	0.16
13:21:19.949 -> -0.28	0.00	0.20
13:21:19.949 -> -0.30	0.00	0.24
13:21:19.949 -> -0.31	0.00	0.27
13:21:19.949 -> -0.32	0.00	0.17
13:21:19.949 -> -0.31	0.00	0.09
13:21:19.949 -> -0.29	0.00	0.14
13:21:19.949 -> -0.31	0.00	0.03
13:21:19.949 -> -0.29	0.00	0.08
13:21:19.949 -> -0.30	0.00	0.12
13:21:19.949 -> -0.31	0.00	0.04
13:21:19.949 -> -0.29	0.00	0.10
13:21:19.983 -> -0.31	0.00	0.02
13:21:19.983 -> -0.29	0.00	-0.05
13:21:19.983 -> -0.28	0.00	-0.11
13:21:19.983 -> -0.26	0.00	-0.03
13:21:19.983 -> -0.28	0.00	-0.09
13:21:19.983 -> -0.26	0.00	-0.01
13:21:19.983 -> -0.28	0.00	0.06
13:21:19.983 -> -0.29	0.00	-0.02
13:21:19.983 -> -0.28	0.00	-0.08
13:21:19.983 -> -0.26	0.00	-0.14
13:21:19.983 -> -0.24	0.00	-0.19

I know it is a bit stupid question, but are you sure that you have connected your motor well?
two phases on two different boards?

If you make an error it might be that it works for open loop somehow but not for the closed loop.

Otherwise I will dig into this this evening and I’ll let you know what I find.
Or if anyone else has some suggestions. I am really not sure what is going on.

Here is open loop with encoder feedback, velocity 1.

13:57:49.612 -> -0.01	-0.05
13:57:50.096 -> -0.05	-0.08
13:57:50.576 -> -0.08	-0.06
13:57:51.053 -> -0.10	-0.04
13:57:51.500 -> -0.13	-0.07
13:57:51.975 -> -0.16	-0.07
13:57:52.451 -> -0.19	-0.05
13:57:52.929 -> -0.20	-0.03
13:57:53.405 -> -0.24	-0.07
13:57:53.881 -> -0.27	-0.07
13:57:54.355 -> -0.31	-0.08
13:57:54.833 -> -0.35	-0.07
13:57:55.307 -> -0.35	-0.02

StepperMotor motor = StepperMotor(PE9, PE11, PE13, PE14, 50, 8);
PE9 goes to in1 board 1 goes to black out (Phase 1 +)
PE11 goes to in2 board 1 goes to green out (Phase 1-)
PE 13 goes to in1 board 2 goes to red out (Phase 2+)
PE 14 goes to in2 board 2 goes to blue out (Phase 2-)

My posts have been limited. Yes I sent a open loop command and print the encoder position and speed.

What are the values you are printing, velocity and position?
You are running open loop control with encoder linked to the motor?

This here has 4 phases driving 4 strings of ca. 6max amp each. 24 amp totalplus the white string of 8 x 350mA LEDS connected in series with 1W 2.7ohm resistor and mosfet on the lamp.
Im thinking to make one in the CAN network wireless. You have to connect the power lines, so another two for can is not that big a deal in my book.

@Juan-Antonio_Soren_E. We are trying to resolve potential issues with the new stepper motor support in this thread. I don’t mean to offend but talking about CAN and LEDs is a bit off topic! I’m guessing you replied in wrong thread.

Its all connected. Assuming you want to use FOC for transportation at some point. Then its a small hub of connected MCU´s with different purposes. For that purpose CAN is the industry go to protocol. All im saying is, now that we are on the brink of this EV wave. Why not have a holistic approch.

@Jonathan_Robson - I’ve finally got around to putting a magnetic sensor on my stepper. I’m seeing slightly strange things, not as bad as you. Nothing conclusive to report yet but just wanted to let you know I’ll be looking into it over the next week.

What I’m seeing is that it works ok in both openloop and closed loop at slow speeds (~5 rad/s) but at higher speeds (~10 rad/s) it has no torque and just goes slower than I would expect from voltage / current I’m throwing at it.

I’m also occasionally hitting some hardware error condition possibly in my mosfet driver which only a PSU off/on will fix (reupload sketch doesn’t fix it).

Anyway I’ll be looking inside the library to see what waveform is being sent to the motor in the next few days.

Here is my setup: nema 17 + storm32 board + as5600


And here is my test sketch:

 #include <SimpleFOC.h>

StepperMotor motor1 = StepperMotor(PB0, PB1, PA7 , PA6, 50);
MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C);

void setup() {

  Serial.begin(115200);
  _delay(750);
  
  sensor.init();
  motor1.linkSensor(&sensor);

  motor1.useMonitoring(Serial);

  motor1.PID_velocity.P = 0.5;
  motor1.PID_velocity.I = 3;

  motor1.voltage_power_supply = 12;
  motor1.voltage_limit = 5;
  motor1.voltage_sensor_align = 3;   
  motor1.controller = ControlType::velocity;
  motor1.foc_modulation = FOCModulationType::SinePWM;
  motor1.init();

  int guess = 0;
  float offset = float(PI / 100.0) * guess;
  // motor1.initFOC(offset, Direction::CW);
  motor1.initFOC();
  
}

float max_speed = 10.0;
long cycle_duration_millis = 6000;

void loop() {

  // Serial.println(sensor.getAngle());
  static long start = millis();
  long now = millis();
  float cycle = float((now - start) % cycle_duration_millis);
  float target_velocity = sin(_2PI * cycle/cycle_duration_millis) * max_speed;

  
  motor1.loopFOC();
  motor1.move(target_velocity);
  static long count = 0;
  if(count % 1000 == 0) {
    motor1.monitor();  
  }
  count ++;
  motor1.monitor();
  
}

Hi Owen,

I’ve also had the driver lockup requiring restart issue, figured it was due to the original problem.

I’ve identified my ‘driver lockup issue’ - it’s not the driver it’s my i2c magnetic sensor holding SDA low. Only happens on resets/uploads. So @Jonathan_Robson this probably means it is not the same lockup as you as I believe you are using an encoder! I’m really not liking the AS5600 - apart from it being cheap it has few redeeming features.

For those using AS5600 the following link has a nice I2C_ClearBus function that can detect if SDA is held low. For me it returns 0 when things are ok and 3 when borked. I might try a more expensive SPI magnetic encoder. I have a few kicking around.

Hey Owen, I had some trouble wen using I2C sensors with STM32 devices as well. At the end I use 4.7KOhm pull-ups the whole time. I even added some to the new SimpleFOCShield for that reason.

The stepper implementation is still experimental so I hope that it will be more stale in future release. :smiley: That being said, the waveforms sent to the motor are two sine waves. I have not implemented the space vector for steppers in this moment.

Works ok means that is works as expected but not really good?
What dies it mean that it has no torque - you can easily stop it?

I also experienced some problems i cannot really explain. When I backdrive the stepper motor it starts fighting me smoothly and then has sudden position jumps (it looks a bit as if it was the open loop, like if it had skipped few steps) but the sensor doesn’t lose the position and the motor returns to the good location afterwards. I am not sure what is causing it really, I have to investigate this more deeply.

Sorry - my description of low torque is not what I meant! I was trying to say low efficiency - but having done a few more tests, that isn’t entirely correct!

So I’ve been testing mostly in closed loop velocity. At less than 5 rad/s it performs more or less perfectly. Low current draw too! After 5 rad/s the current climbs rapidly and it’s speed tops out at 7 to 8 rad/s at which point it is drawing 5x the current of that at 5 rad/s. These steppers typically have a max speed of 700-800rpm. I’m maxing out at 8 rad/s (~76rpm). So the weird behaviour is low top speed and poor efficiency above 60rpm.

On a possible tangent - I’d like to understand a bit more about how current flows. In this diagram I’m just showing one phase duty_cycle1A=red and duty_cycle1B=purple:

If we look at the 1st red peak - we have a PWM of ~0.166 and 0.0 on 1A and 1B (first phase). I think 0.0 equates to -Vsupply and 0.17 would be approx -0.66 Vsupply (and 0.5 would be zero and 1.0 would be +Vsupply). So at red peak we have a potential difference of 0.33*Vsupply. Sound right?

Assuming the above is right (I expect its not!) what I don’t get is the fact that we are dealing with chopped PWM - what happens if PWM of both sides of phase 1 are not aligned? Does that mean there are misalignment times when there is no path for the current to flow?

Hey @Owen_Williams,
I’m sorry for a bit of delay but I wanted to take some time to answer this question properly.

Two phase stepper motors supported by this library have following structure:

You can see that from the images it is possible to achieve positive and negative voltages on each phase separately. This was not possible for BLDC motors, for BLDC motors our goal was to set a certain voltage in between phases A,B and C. For stepper motor can set any voltage in between V_power_supply and -V_power_supply to each phase A and B.

Now the PWM modulation implemented in this library is the sinusoidal modulation where we only keep Inverse park transformation and discard the Clarke transformation.
Inverse park transformation for stepper motor is:

U_alpha = - Uq* sin(angle)
U_beta = Uq* cos(angle)

where Uq is the q component of the volatge we want to set to the motor and angle is the current electrical angle of the motor.

So for the certain Uq which is fixed, we will get U_alpha and U_beta as negative sine and cosine waves. So from the image which follows and the one before we can conclude that the pattern of the voltages applied to the 1A,2A,1B and 2V is as follows

Now if you look closely, this is exactly what you have plotted in your message.
I hope this makes things a bit more clear. :smiley:

Now in terms of problematics that have arisen for stepper motors I think I have an idea why the cutoff in velocity happens.

For each electrical rotation of the stepper motor, for each time the electrical angle goes form 0 to 360 degrees ( this happens pole_pairs number for one rotor revolution ), the absolute minimum number of FOC algorithm executions (the algorithm calculating sine and cosine waves) is 4.
Which means that for nema 17 motor with 50 pole pairs:

N = 50 x 4 = 200

This means, in order for your rotor to make a full revolution you need to execute Arduino loop() minimally 200 times. And if you are using an I2C sensor it is likely that your loop time is close to 1ms.

This basically means that one motor rotation cannot be faster than 200ms. or in other words 5 rotations per second. This doesn’t explain fully the behavior we are seeing but in my opinion this is the problem we are facing.
For Arduino UNO, with 500ppr encoder I cannot exceed velocity 5-6 rad/sec. If I plug the same setup to Nucleo board I can produce velocities well above 30rad/s.
I will make one proper video soon to demonstrate this.

Therefore @Owen_Williams, can you please tell us the average loop time you are having with you setup to see if this explains it. :smiley:

3 Likes

Thanks @Antun_Skuric - those diagrams are exactly what I saw!

I have replaced my i2c with spi (AS4047u). I’ve managed to get a closed loop above 50 rad/s (~500 rpm) but had to do some surgery on the code! I’m basically running loopFOC() as fast as possible but only calling sensor.getVelocity() at about 200Hz (loopFOC is just given the last shaft_velocity for when sensor.getVelocity() is skipped. This approach has the effect of smoothing velocity. I tried playing with Tf but couldn’t get it tuned.

This sensor frequency approach to velocity smoothing is something I think we should investigate. Ironically faster chips have more velocity issues. I’m sure you’ve seen raw velocity outputs like this 0, 0, 0, 0, 700, 0, 0, 0, 0, 0, 700 where the zeros are caused by angle - prev_angle == 0 and the 700 is cause by angle - prev_angle == 1 which gets divided by a very small time diff.

This lots-of-zeros-with-occasional-high velocity readings is incredibly difficult to smooth. My intuition says we should be sampling less frequently i.e. to get <10% variation we at least need to ‘wait’ for 10 units of sensor precision to tick over. At low shaft velocity a low sample frequency (e.g. 50hz) really works well, at higher shaft velocities higher frequencies (1Khz) may make sense.

I haven’t fully worked out how having separate foc and sensor loop frequencies might work or when it might be preferable over the normal low pass filter. More testing!

Here is a vid of it doing 50 rad/s. I’m going to see if I can get it running at 100+rad/s (I think it did 130 rad/s when I forgot to put a voltage limit on it!!)

Hey @Owen_Williams,

This definitely makes sense!
Did you try to separate the motor.loopFOC() and motor.move(). Run loopFOC() in each loop() and move() with much lower frequency. This should do more or less the same thing.

I have done the same thing for my inverted pendulum project. You can see the code in:

1 Like

I had much better results with my robots slowing down motor.move(), but I was doing position control.
Do you think there might be a way to calculate the optimal frequency for motor.move()?