update_v1.1: Fix motor output drift, restructured code