Motion planner code

[thalesmaoa]

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