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