Fix a PID bug and finished the motor control