Cleaning the code, added new control folder for future function in common for all the controllers: saturation, rate limiter, trajectory generation. TODO rewrite the controllers with the functions