Added in test x, y, and yaw P controllers