removed scaling on the P controller since it was not dealing with velocities.