private IEnumerator <bool> RotateAllTo(float angle, RotationSpeed speed)
{
Begin(m => m.RotateTo(angle, speed));
yield return(true);
while (RunModules(enumeratorPerModule))
{
yield return(true);
}
}
/// <summary>
/// Rotates module smoothly to the specified angle.
/// </summary>
/// <remarks>
/// The iterator yields true while rotation is ongoing, then yields false thereafter.
/// It is non-terminating; stepping after completion merely checks invariants and enforces
/// rotor lock.
/// </remarks>
public IEnumerator <bool> RotateTo(float targetAngleDegrees, RotationSpeed speed)
{
if (IsModuleViable())
{
var stopwatch = new Stopwatch();
var initialAngleDegrees = rotorPair.CurrentAngleDegrees;
var thisRotationLimits = limits.ClampRotation(initialAngleDegrees, targetAngleDegrees, 1);
UnlockAndPrepareForRotate(thisRotationLimits);
var sqrtSpringConstant = Math.Sqrt(speed.SpringConstant);
// Indicate that we're ready to start.
yield return(true);
while (IsModuleViable())
{
var remainingDegrees = targetAngleDegrees - rotorPair.CurrentAngleDegrees;
var currentVelocityDps = rotorPair.TargetVelocityDegreesPerSecond;
if (IsRotationComplete(remainingDegrees, currentVelocityDps))
{
break;
}
var elapsed = stopwatch.GetElapsedSeconds();
// Critically-damped spring:
var acceleratingForce = remainingDegrees * speed.SpringConstant;
var deceleratingForce = -currentVelocityDps * 2 * sqrtSpringConstant;
var force = acceleratingForce + deceleratingForce;
// Err on the side of smaller time steps.
var expectTimeStep = Math.Min(RotationSpeed.TimeStepSeconds, elapsed);
var accelerationNextSecond = force;// MathHelper.Clamp(force, -specification.MaximumAcceleration, specification.MaximumAcceleration);
rotorPair.TargetVelocityDegreesPerSecond += (float)(accelerationNextSecond * expectTimeStep);
stopwatch.Reset();
yield return(true);
}
}
// Complete. Enforce limits and lock.
while (true)
{
ForceLock();
yield return(false);
}
}
