Finalized mass adaptive controller
Modify x-y controller with no small angle assumption
Problem: big delay in acceleration, observed as 0.6-1s