refactored speed control methods
first attempt to add MPU6050 for yaw control