Improved RCS control for Docking

At least it works better when I test it
Not perfect yet