motor control working, need to fine-tune gains and velocity calculation