simplify, and fix motor control