public void Reset()
{
for (int i = 0; i < gyros.Count; i++)
{
IMyGyro g = gyros [i];
if (g == null)
{
gyros.RemoveAtFast(i);
continue;
}
g.GyroOverride = false;
}
pitchPID.Reset();
yawPID.Reset();
rollPID.Reset();
}
public void Turn(MatrixD Reference, Vector3D target)
{
if (Reference == MatrixD.Zero)
{
return;
}
MatrixD orientationMatrix = MatrixD.Identity;
orientationMatrix.Translation = Reference.Translation;
Vector3D orientationForward = Controller.WorldMatrix.Forward + Controller.WorldMatrix.Down + Controller.WorldMatrix.Left;
orientationForward.Normalize();
orientationMatrix.Forward = orientationForward;
Vector3D orientationUp = Reference.Forward;
orientationUp -= VectorHelpers.VectorProjection(orientationUp, orientationForward);
orientationUp.Normalize();
orientationMatrix.Up = orientationUp;
Vector3D OrientationRight = orientationForward.Cross(orientationUp);
orientationMatrix.Right = OrientationRight;
var gravDir = Controller.GetNaturalGravity();
gravDir.Normalize();
double yawAngle, pitchAngle, spinAngle;
GravAngle = VectorHelpers.VectorAngleBetween(gravDir, orientationMatrix.Forward);
if (target != Vector3D.Zero && GravAngle < Math.PI * 0.1)
{
Vector3D TargetDir = target - orientationMatrix.Translation;
TargetDir.Normalize();
var projectedTargetUp = TargetDir - VectorHelpers.VectorProjection(TargetDir, orientationForward);
spinAngle = -1 * VectorHelpers.VectorAngleBetween(orientationMatrix.Up, projectedTargetUp) * Math.Sign(orientationMatrix.Left.Dot(TargetDir));
}
else
{
spinAngle = 0;
SpinPID.Reset();
}
TrigHelpers.GetRotationAngles(gravDir, orientationMatrix.Forward, orientationMatrix.Left, orientationMatrix.Up, out yawAngle, out pitchAngle);
TrigHelpers.ApplyGyroOverride(PitchPID.Control(pitchAngle), YawPID.Control(yawAngle), SpinPID.Control(spinAngle), Gyros, orientationMatrix);
}
public void Update()
{
LifeTimeTicks++;
// Startup Subroutine
if (LifeTimeTicks == 1)
{
foreach (var engine in Drive.HoverEngines)
{
engine.AltitudeMin = 1;
engine.PushOnly = true;
}
}
if (LifeTimeTicks == 60)
{
foreach (var engine in Drive.HoverEngines)
{
engine.AltitudeMin = 2;
}
}
if (LifeTimeTicks == 120)
{
foreach (var engine in Drive.HoverEngines)
{
engine.AltitudeMin = 7;
}
}
if (LifeTimeTicks % kRunEveryXUpdates == 0)
{
Vector3D PlanetDir = PlanetPos - Controller.WorldMatrix.Translation;
PlanetDir.Normalize();
// Orient Self
Drive.Turn(Gats.Count > 0 ? Gats[0].WorldMatrix : Controller.WorldMatrix, target);
// Aim Turret
if (TurretRotor != null)
{
if (target != Vector3D.Zero && Drive.GravAngle < Math.PI * 0.1)
{
var TurretAngle = TrigHelpers.FastAsin(Gats[0].WorldMatrix.Forward.Dot(PlanetDir));
Vector3D TargetDir = target - Gats[0].WorldMatrix.Translation;
var targetDist = TargetDir.Length();
TargetDir.Normalize();
var TargetAngle = TrigHelpers.FastAsin(TargetDir.Dot(PlanetDir));
var angleDiff = TargetAngle - TurretAngle;
if (VectorHelpers.VectorAngleBetween(Gats[0].WorldMatrix.Forward, TargetDir) < 0.05 && targetDist < 800)
{
Fire();
}
TurretRotor.TargetVelocityRPM = (float)TurretPID.Control(angleDiff);
}
else
{
TurretPID.Reset();
TurretRotor.TargetVelocityRPM = 0;
}
}
// Check your fire
fireTicks--;
if (fireTicks == -1)
{
foreach (var gat in Gats)
{
TerminalPropertiesHelper.SetValue(gat, "Shoot", false);
}
}
// Check Movement
if (Destination != Vector3D.Zero)
{
Drive.Drive(Destination);
if (Drive.Arrived)
{
Destination = Vector3D.Zero;
Drive.Flush();
}
}
}
}
public void Reset()
{
X.Reset();
Y.Reset();
Z.Reset();
}
