Measurment fixed
Forward, backward, and stop works