private Vector3 GetCurrentLegPosition(LegConfiguration legConfig) { MotorGoalPositions positions; lock (_driver.SyncLock) { float coxa = _driver.GetPresentPositionInDegrees(legConfig.CoxaId); float femur = _driver.GetPresentPositionInDegrees(legConfig.FemurId); float tibia = _driver.GetPresentPositionInDegrees(legConfig.TibiaId); positions = new MotorGoalPositions(coxa, femur, tibia); } return(CalculateFkForLeg(positions, legConfig)); }
private bool CenteringDone() { if (_centering) { lock (_driver.SyncLock) { var centered = AreClose(_driver.GetPresentPositionInDegrees(HorizontalMotorIndex), HorizontalCenter) && AreClose(_driver.GetPresentPositionInDegrees(VerticalMotorIndex), VerticalCenter); _centering = !centered; return centered; } } return true; }