public Vector3D Update(Vector3D error, TimeSpan sincelastupdate)
{
return(new Vector3D(
X.Update(error.X, sincelastupdate),
Y.Update(error.Y, sincelastupdate),
Z.Update(error.Z, sincelastupdate)));
}
private double PointRotorAtVector(IMyMotorAdvancedStator rotor, Vector3D currentDirection, Vector3D rotorPlaneNormal, Vector3D targetDirection)
{
// Get max RPM for a rotor
float maxRPM = rotor.GetMaximum <float>("Velocity");
// maxRPM rad
double errorScale = Math.PI * maxRPM;
// Grab target vector projected onto the plane of the engine
Vector3D targetOnPlane = targetDirection - targetDirection.Projected(rotorPlaneNormal);
// Normalized (set length to 1)
var targetNormalized = targetOnPlane.Normalized();
var currentNormalized = currentDirection.Normalized();
// Calculate angle between target and current direction, in [-pi to pi]. The first value is a triple product
double angle = Math.Atan2(rotorPlaneNormal.Normalized().Dotted(targetNormalized.Crossed(currentNormalized)), targetNormalized.Dotted(currentNormalized));
if (angle > Math.PI)
{
angle -= 2 * Math.PI;
}
else if (angle <= -Math.PI)
{
angle += 2 * Math.PI;
}
// Find rotation speed value using a PID
var rotVal = pid.Update(angle * errorScale);
//logger.Log($"angle: {angle} rotVal: {rotVal}");
// The rpm value must be -maxRPM <= rpm <= maxRPM
double rpm = MathHelper.Clamp(rotVal, maxRPM * -1, maxRPM);
rotor.TargetVelocityRPM = (float)rpm;
return(angle);
}