Rewritten motor_control and some other stuff