Shortest path returning to zero point in a position loop?

Hi everyone.I’m doing a project which SimpleFOC drives a BLDC motor in positon mode.
What I’m trying to do is this:
Say the current motor angle is 6rad and new command is 0,instead of going back all the way around and return to zero, the motor takes the shortest path which is to move forward for around 0.28rad and resets its angle to zero.
What kind of code should I implement to achieve this effect?

With this done, does it mean with a input signal repeatly ramping up from 0 to 6.28 the motor would look like infinitely rotates toward one direction but the target angle wouldn’t get too large to avoid any possible problems.

I’m kind of new to this programming thing so could anyone shine a light on this?Any help is appreciated,thanks!

I posed this question on stack exchange a handful of years ago with this post if you want the full explanation.

You basically do the calculations with an additional 2pi (so there is no 2pi → 0 radian crossover) then modulo it (remainder) to get back in terms you started with.

I can convert it into C for your situation if the post doesnt make sense

Thank you so much for such quick answer sir! I will look into the post right away