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.