Add working 2DOF arm angle solver