Forgot a file; Now works with servocontroller