Motion Controller nearly working, yaw issues