/// <summary>
/// Compares the current angle of the stator to the target angle. Returns true if there is
/// no target angle set. (Hopefully temporary if Keen fixes the mismatched units between
/// getters and setters.)
/// </summary>
public bool CompareTargetAngle()
{
if (properties.targetAngleRad == StatorProperties.INFINITE_ANGLE_RADIANS)
{
return(true);
}
// Normalize to [-180, 180]
float normCurrentAngleDeg = StatorProperties.NormalizeDeg(StatorProperties.RadToDeg(stator.Angle));
// Normalize to [-π, π]
float normCurrentAngleRad = StatorProperties.NormalizeRad(stator.Angle);
double twoPi = 2 * Math.PI;
float diffRad = (float)((Math.Abs(normCurrentAngleRad - properties.targetAngleRad) + twoPi) % twoPi);
float diffDeg = (Math.Abs(normCurrentAngleDeg - properties.targetAngleDeg) + 360) % 360;
if (diffRad < 0.00008f || diffDeg < 0.0035f)
{
return(true);
}
return(false);
}
/// <summary>
/// Runs the stator to match the target angle in its properties.
/// </summary>
public Func <IEnumerator <bool> > RunManagedMovement()
{
int rotationDirection = 1;
float normCurrentAngle = StatorProperties.NormalizeDeg(StatorProperties.RadToDeg(stator.Angle));
// Get the shortest direction of travel
if ((properties.targetAngleDeg - normCurrentAngle + 360) % 360 > 180)
{
rotationDirection = rotationDirection * -1;
}
// Check if the shortest direction intersects the limits
if (properties.upperLimitRad != StatorProperties.INFINITE_ANGLE_RADIANS ||
properties.lowerLimitRad != -StatorProperties.INFINITE_ANGLE_RADIANS)
{
// Check for segment intersections
if (Intersect(normCurrentAngle, properties.upperLimitDeg, properties.lowerLimitDeg) ||
Intersect(properties.targetAngleDeg, properties.upperLimitDeg, properties.lowerLimitDeg) ||
Intersect(properties.upperLimitDeg, normCurrentAngle, properties.targetAngleDeg) ||
Intersect(properties.lowerLimitDeg, normCurrentAngle, properties.targetAngleDeg))
{
rotationDirection = rotationDirection * -1;
}
}
// Apply the rotation angle
stator.TargetVelocity = Math.Abs(StatorProperties.RpmToRads(stator.TargetVelocity)) * rotationDirection;
// Get the absolute distance from the current angle to the target angle
float diff = Math.Abs(StatorProperties.RadToDeg(stator.Angle) - properties.targetAngleDeg);
float limit = 0;
// Set temporary limits to avoid overshoots
if (stator.TargetVelocity > 0f)
{
stator.SetValueFloat("UpperLimit", properties.targetAngleDeg);
// Stop the stator from having a starting angle greater than 360 degrees
// from the target angle
if (diff > 360)
{
// Set a limit that would make the starting angle be out of bounds and
// force the game to update starting angle within tighter limits
limit = StatorProperties.RadToDeg(stator.Angle) + 181;
stator.SetValueFloat("LowerLimit", limit);
}
}
else
{
stator.SetValueFloat("LowerLimit", properties.targetAngleDeg);
if (diff > 360)
{
limit = StatorProperties.RadToDeg(stator.Angle) - 181;
stator.SetValueFloat("UpperLimit", limit);
}
}
// Turn the stator
stator.SafetyLock = false;
stator.Enabled = true;
return(Runner);
}
