private float GetDistanceFromGoal(float angle)
{
float forwardDistance;
float goalAngle = MathHelper.WrapAngle(settings.servo.goal);
if (goalAngle > 0)
{
if (angle > goalAngle)
forwardDistance = angle - goalAngle;
else if (angle > 0)
forwardDistance = MathHelper.TwoPi - goalAngle + angle;
else //if (angle <= 0)
forwardDistance = MathHelper.TwoPi - goalAngle + angle;
}
else
{
if (angle < goalAngle)
forwardDistance = MathHelper.TwoPi - goalAngle + angle;
else //if (angle < 0)
forwardDistance = angle - goalAngle;
//else //if (currentAngle >= 0)
// return angle - myMinimumAngle;
}
return forwardDistance > MathHelper.Pi ? MathHelper.TwoPi - forwardDistance : -forwardDistance;
}
}