[11_MotionControl] Fix bugs of Nifu and switching side