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