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;
}