Motion planner code

I am using simpeFOC with the STM32G431. My application requires very fast movements in position mode (going back and forth between 2 angles).
I wasn’t able to achieve that with the standard angle PID in simpleFOC.
Increasing the Kp of the angle PID gives me faster accelerations, but the axis rotates badly because of the high gain. there is no setting that can achieve my demands, and I tried everything.
That led me to believe that only a motion planner will be the correct route to achieve this. I am sure it can be done, because the speed loop works flawlessly - the speed is being maintained within a very small percentage.
I want to be able to enter the acceleration, deceleration, profile speed and the target position of course.
Some of my movements require triangular movement, not speed trapezoidal (where the setpoint speed is reached for a very short time and then decelerates)

Can anyone point me to a good motion planner code that I can incorporate into SimpleFOC?


Just began tuning.

I’m doing a similar thing, my solution went to make adaptive pid tunings, related to the specific function (eg: relaxed on small gaps, aggressive on quick movements)

Check here

My relaxed angle pid tunings are like p=0.6, i=0.2 while aggressive are p=60, i=2.0

Your approach looks classy, and more appropriate, I’m following.


1 Like

Hey @Geronimo,

We have had a discussion some time ago about adding feedforward inputs to the simplefoc.
I think it would be a good idea, it would enable you to use the external planning algorihtm without the need to rewrite the whole control part of the library.

Maybe we will attack this problem as well and make a SimpleFOC motion planner. I am not sure at the moment. Probably trapezoidal velocity profile based approach.

I’ve seqrched quicky and it seems that there is an arduino library that might do the trick:

If you wanna test it and if you get some good results, please do let me know. If it works well, we might do a collaboration with them maybe. :smiley:



@Antun_Skuric is correct. I don’t think there is much to improve without feed-forward.

First is that, for motion control, one thing must be clear. There is no step response. If such thing exist, it means that power source must be infinite. That is not reasonable. There always must be a ramp, not a step.

Second is that feed-forward is mandatory for a good control. When the entire error is put in feedback, the control will never work correctly. Feedback is for dynamic, feed-forward is for standstill. If, BEMF is know, there is a information to be told to the control loop that it is missing.

For example, we have a motor with a known linkage flux (with some variations called kv), e.g. if the motor is feed with 4Vac, it is known that it must run at 3500rpm. In reality, that doesn’t happen, because there are loads and transient. This is where feedback comes to place.

I will try to implement two speed feed-forward. The first one is also known as explicit speed declaration (image bellow), the second one is just a feed-forward.

Lorenz, Robert D., Thomas A. Lipo, and Donald W. Novotny. “Motion control with induction motors.” Proceedings of the IEEE 82.8 (1994): 1215-1240.

The explicit speed declaration makes infinite bandpass for position, which means that will probably work even in a 8-bit arduino.

1 Like

Thanks for the information guys.
So basically you are saying,
current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity) + FEEDFORWARD;

I don’t understand how it relates to the motion planner. In my understanding the motion planner creates the relevant velocity or position based on the current time (after the trapez was created) and it is just setpoint position/ speed to the regular PID we already use inside SimpleFOC.
What do I miss here?

I’m not sure if I understood you question.

I want to know how to implement the feedforward into SimpleFOC.

In the meantime I’m going to try to try to implement LQR for the quick, defined, angle movements

You can start from explicit speed declaration. Basically, integrate position an sum after the position PI as presented in the picture above.

The concepts being discussed are more advanced ones. I can’t really explain it, but I can answer your other question:

Yes, to follow position trajectories (like trapezoids) you can definately achieve it by adding an outer control loop above the normal SimpleFOC loop, which updates the set points and limit values to make the system follow the path you want.

The feedforward sounds very cool, but I’m not sure it’s needed to achieve your goal.

This is exactly what we need to make a FOC stepper driver talk to Klipper. I guess a buffer of sorts holds the incoming angle set points, acceleration, feedrate, de-acceleration. In the case of Klipper integration, then it’s all tied around time-synchronization. So if we can make the FOC stepper driver execute the desired angles within the target time, with a prefixed acceleration etc. then a multi axis machine can be synced through Klipper.

Any progress @Geronimo ?

@Juan-Antonio_Soren_E unfortunately no, The only scenario I see right now is to dynamically limit the PID_velocity.limit as a function of actual position.

I think @runger is right then. The planner needs to be on top of the FOC algorithm, tightly controlling the torque, movements etc. entwined

Future Request! SimpleFoc (Field Oriented Control) stepper strategy, not step/dir in time but movement in space and time - Features - Klipper

The Outer_loop need to look ahead like its done in Marlin, Smoothieware etc. Those target positions, timestamped or scheduled, should then be translated to FOC movement, with a dynamic factor as well to counter inertia. Only there is no conversion to step/dir needed, since each “main_board” is a sub_driver controlling the stepper or BLDC (servo) directly

Different types of moves for multi_axis machines based on G_code:

By convention, most G-code generators use G0 for non-extrusion movements (those without the E axis) and G1 for moves that include extrusion. This is meant to allow a kinematic system to, optionally, do a more rapid uninterpolated movement requiring much less calculation.

The control PID loop for path in time must be in the fraction of an angle order. The controller should know if its “running late” down to a eg. 0.001mm. This is where the human mind and the silicon precision collide.

On another note, the communication and buffering and all that may interfere with the FOC loop, unless it’s a dual core MCU.

So you are saying that only the ESP32 can do it. (running the FOC loop and the motion planner on the same IC).
I don’t know, maybe. I think 180MHz STM32 can definitely do it, provided it’s not busy handling fast encoder interrupts or tight and frequent serial commands.

Im just saying that I know how the Commander serial interface can make the FOC loop a bit laggy, when monitoring, but that may be because USB serial implementation is not utilizing the full USB potential with DMA and such. For sure DMA frees up the MCU, but DMA is quite MCU specific and need a lot of hardcore coding to registers. Or maybe not alot, but some and different MCUs has different USB capabilities.

In the case of a multi_axial machine there would be a significant amount of data transfer, Gcode interpretation, but maybe 10-15 moves is enough for the buffer ?

Like always its important to be specific. In the case of Klipper commands (lets call it that, moves with a timestamp), There would be a machine defined space divided into eg. angle fractions. The typical equivalent would be axis in mm divided into steps/mm. All the Gcode translation into the angle format should take place in Klipper (On a Linux install on a PC/laptop or RPi) so, the moves should be in the desired format, with a timestamp. In order for a timestamp to make any sense, the sub_drivers has to be synchronized. I believe Klipper has a way to do that.

Compared to a traditional “main_board” controlling multible axis from one MCU, you can argue that it would be a multi core machine, since each sub_driver “only” has to focus on one axis and would not receive data/moves related to the other axis, since its sorted by Klipper.

It would seem that Klipper3D provides all the necessary buffer and MCU implementation for their specific HOST protocol. We would need to do some minor adjustments for it to translate to angles. Hmmm :thinking:

Lets not get ahead of ourselves… it might be true, but ad hoc I would have said that while a 8bit ATMega will probably not be sufficient (as its barely powerful enough to run SimpleFOC without motion planning), a 32 bit MCU will probably be sufficient.
Monitoring overhead is proportional to the FOC iteration speed (reduced by motion_downsample), but setting targets can generally run a lower speed, so I would expect the communications overhead to be less than that of monitoring.

Yes you are right. What I did is to print integers instead of floats in the serial monitor. It gave me much better performance.


Can someone with a bit more hands on SimpleFoc Code give a answer here in Klipper forum, regarding how the optimal approach, for a SimpleFoc merge, would make best sense. From a FOC perspective…