Removed motor/servo compensation (needs PID)