ARM: Modify simulation and controller to reflect real servo angles