#region Script
/******************************************************************************
VERCION_CODE = 6.6 中文版;
//导弹必须使用合并方块连接才能发射
//导弹必须使用合并方块连接才能发射
//导弹必须使用合并方块连接才能发射
//脚本分为两部，火控雷达和导弹
//1，在自定义里填写是否是导弹或火控雷达,完毕重置代码
//2，如果是导弹重置完代码后需要运行 label=xxx 区分导弹型号(或类型)
执行这条命令导弹必须是子网格或独立网格。
//补充导弹指令使用
功能：切换到锁定模式（lock），用于搜索并锁定目标。
指令："lock"
功能：切换到炮塔模式（turret），用于炮塔自动搜索目标。
指令："turret"
功能：发射导弹（前提是已锁定目标）。
指令："fire"
功能：重置导弹状态，清除所有准备好的导弹列表。
指令："reset"
功能：设置导弹标签，导弹必须是子网格或独立网格。
指令："label=xxx"
功能：设置远程密钥，用于与其他火控系统通信。
指令："RemoteKey=1234567890"
功能：复制GPS信息运行，洲际导弹功能，速度低于500，不可用
指令：从GPS列表复制的信息
******************************************************************************/
Missile _missile;
string FireControllerId;
IMyBroadcastListener _broadcastListener;
public enum STATE : byte { ERROR = 1, READY = 2, ATTACK = 3, LOCKON = 4, UNLOCK = 5, SEARCH = 6, RECHARGE = 7, DETACH = 8, CHECK = 9, TURRET = 10, NAVIGATION = 11, }
public enum AttackMode : byte { Center = 1, AimPosition = 2 }
public enum NavMode : byte { Standard = 1, Vector = 2, }
public Program()
{
Me.GetSurface(0).ContentType = ContentType.NONE;
Me.GetSurface(1).ContentType = ContentType.NONE;
string CustomData = Me.CustomData;
string[] splitData = CustomData.Split('\n');
if (splitData.Length != 15)
{
Missile.RsetConfig(Me);
Me.CustomName = "*Fire Control";
}
else if (!Missile.SetConfig(Me, splitData))
{
FireControllerId = "" + Me.EntityId;
_broadcastListener = IGC.RegisterBroadcastListener(FireControllerId);
_broadcastListener.SetMessageCallback(FireControllerId);
Me.CustomName = "*Fire Control";
}
Missile._state = STATE.ERROR;
Runtime.UpdateFrequency = UpdateFrequency.Update100;
}
public void Main(string argument, UpdateType updateSource)
{
argument = argument.Trim();
if (_missile == null)
{
_missile = new Missile(this);
return;
}
if ((updateSource & UpdateType.IGC) > 0)
{
while (_broadcastListener.HasPendingMessage)
{
MyIGCMessage message = _broadcastListener.AcceptMessage();
string[] splitdata = message.Data.ToString().Split('=');
if (_missile == null) return;
switch (splitdata[0])
{
case "Request":
if (!_missile._reqDictionary.ContainsKey(splitdata[1]))
{
_missile._reqDictionary.Add(splitdata[1], Missile._timetick);
IGC.SendBroadcastMessage(splitdata[1], "Received=");
_broadcastListener.SetMessageCallback();
}
break;
case "Received":
_missile.OpenRadioAntenna(Missile.AntennaOpen);
Missile._messageTick = -1;
break;
case "Target":
_missile.AnalysisTarget(message.Data.ToString(), UpdateType.IGC);
_broadcastListener.SetMessageCallback();
Missile._messageTick = 0;
break;
case "ReadyMissile":
long missileId; long.TryParse(splitdata[1], out missileId);
if (!_missile._missileList.ContainsKey(splitdata[1]))
_missile._missileList.Add(splitdata[1], Missile._timetick);
_missile.RenderLCD();
break;
case "RemoveMissile":
if (_missile._missileList.ContainsKey(splitdata[1]))
_missile._missileList.Remove(splitdata[1]);
_missile.RenderLCD();
break;
case "AddFireController":
if (!_missile._remoteFireController.ContainsKey(splitdata[1]))
_missile._remoteFireController.Add(splitdata[1], Missile._timetick);
foreach (var m in _missile._missileList) { IGC.SendBroadcastMessage(splitdata[1], "ReadyMissile=" + m.Key); }
break;
case "RemoveFireController":
if (_missile._remoteFireController.ContainsKey(splitdata[1]))
_missile._remoteFireController.Remove(splitdata[1]);
break;
case "RemoteFireTarget":
_missile.RemoteFire(IGC, message.Data.ToString().Replace("RemoteFire", ""));
break;
}
}
return;
}
if (!String.IsNullOrEmpty(argument))
{
if (argument.ToLower().Equals("lock") || argument.ToLower().Equals("turret"))
{
if (Missile._state == STATE.SEARCH || Missile._state == STATE.LOCKON || Missile._state == STATE.TURRET)
{
Missile._state = STATE.READY;
_missile.SelectSounds("SoundBlockObjectiveComplete", 3);
}
else if (Missile._state == STATE.READY)
{
Missile._state = argument.ToLower().Equals("lock") ? STATE.SEARCH : STATE.TURRET;
_missile._cameras.InitFireControllerCameras(argument.ToLower().Equals("lock") ? 30000 : 2000);
_missile.SelectSounds("SoundBlockAlert2", 300);
_missile.RenderLCD();
Runtime.UpdateFrequency = UpdateFrequency.Update10;
}
}
else if (argument.ToLower().Equals("recharge"))
{
if (Missile._state == STATE.READY)
{
Missile._state = STATE.RECHARGE;
Runtime.UpdateFrequency = UpdateFrequency.Update100;
}
}
else if (argument.StartsWith("ReadyMissile"))
{
_missile.AddReadyMissile(argument, IGC);
}
else if (argument.ToLower().StartsWith("target"))
{
long targetId = _missile.AnalysisTarget(argument, UpdateType.Terminal);
if (targetId != 0)
{
_broadcastListener = IGC.RegisterBroadcastListener(Me.EntityId + "");
_broadcastListener.SetMessageCallback(Me.EntityId + "");
Missile._state = STATE.CHECK;
Runtime.UpdateFrequency = UpdateFrequency.Update1;
}
}
else if (argument.StartsWith("RemoteKey="))
{
if (string.IsNullOrEmpty(_missile.RemoteKey))
{
_missile._missileList.Clear();
_missile.RenderLCD();
_missile.RemoteKey = argument.ToLower().Replace("remotekey=", "");
IGC.SendBroadcastMessage(_missile.RemoteKey, "AddFireController=" + Me.EntityId);
}
else
{
IGC.SendBroadcastMessage(_missile.RemoteKey, "RemoveFireController=" + Me.EntityId);
_missile.GetReadyMissiles();
_missile.RemoteKey = "";
}
}
else if (argument.ToLower().StartsWith("ready"))
{
_missile.AddReadyMissile(argument, IGC);
}
else if (argument.ToLower().StartsWith("speed"))
{
Missile.ChangeSpeed(Me, argument);
}
else if (argument.ToLower().StartsWith("label="))
{
Missile.SetAllLabel(this, argument);
}
else if (argument.ToLower().Equals("reset"))
{
_missile.GetReadyMissiles();
}
else if (argument.ToLower().Equals("fire"))
{
if (string.IsNullOrEmpty(_missile.RemoteKey))
Missile._fire = true;
else
_missile.Fire(IGC);
}
else if (argument.ToLower().Equals("attack_switch"))
{
if (Missile._AttackMode == AttackMode.Center)
{ Missile._AttackMode = AttackMode.AimPosition; }
else
{ Missile._AttackMode = AttackMode.Center; }
}
else if (argument.ToLower().StartsWith("gps"))
{
_missile.SplitGPSInfo(IGC, argument);
}
return;
}
switch (Missile._state)
{
case STATE.ERROR:
_missile.initMissile();
break;
case STATE.READY:
_missile.Ready();
break;
case STATE.LOCKON:
_missile.KeepLockTarget(IGC, _broadcastListener);
break;
case STATE.ATTACK:
_missile.AttackTarget(IGC);
break;
case STATE.UNLOCK:
_missile.RequestAttackTarget(IGC);
break;
case STATE.SEARCH:
_missile.Raycast(IGC);
break;
case STATE.TURRET:
_missile.TurretSearch(IGC);
break;
case STATE.RECHARGE:
_missile.Recharge();
break;
case STATE.DETACH:
_missile.Detachment();
break;
case STATE.CHECK:
_missile.CheakMissile();
break;
case STATE.NAVIGATION:
_missile.NavigationTo();
break;
}
Missile._timetick++;
Echo(Missile._state.ToString());
Echo(Missile._timetick + "");
_missile.PlaySound();
}
#region Missile
class Missile
{
public MYCameras _cameras;
public MYLCD _mylcd;
private MYGyro _gyro;
private MYThrusts _thrusts;
private MyGridProgram _program;
private List<IMyTerminalBlock> terminalBlocks = new List<IMyTerminalBlock>();
private List<IMyShipController> _shipControllers = new List<IMyShipController>();
private List<IMyShipMergeBlock> _mergeBlocks = new List<IMyShipMergeBlock>();
private List<IMyShipConnector> _cnnBlocks = new List<IMyShipConnector>();
private List<IMyRadioAntenna> _radioBlocks = new List<IMyRadioAntenna>();
private List<IMyGasTank> _gasTanks = new List<IMyGasTank>();
private List<IMyWarhead> _warheadBlocks = new List<IMyWarhead>();
private List<IMySensorBlock> _sensorBlocks = new List<IMySensorBlock>();
private List<IMySoundBlock> _soundBlocks = new List<IMySoundBlock>();
private List<IMyProjector> _projector = new List<IMyProjector>();
private IMyShipController _cockpit;
public string RemoteKey = "";
public Dictionary<string, int> _missileList = new Dictionary<string, int>();
public Dictionary<string, int> _remoteFireController = new Dictionary<string, int>();
public Dictionary<string, int> _reqDictionary = new Dictionary<string, int>();
private bool IsArmed = false;
public static STATE _state = STATE.ERROR;
public static STATE _lastState = STATE.SEARCH;
public static int _lastFireTick = -60;
public static int _timetick = 0;
public static int _messageTick = 0;
public static AttackMode _AttackMode = AttackMode.Center;
public static NavMode _NavMode = NavMode.Standard;
private Vector3D DetachPosition;
private Vector3D LaunchVelocity;
public static double FlightSpeed = 200;
public static double FlightHeight = 500;
public static double DetachDistance = 100;
public static bool IsMissile = false;
public static string VERCION_CODE = "VERCION_CODE=6.6";
public static string MissilePwd = "";
public static string LabelLCD = "[ADS|LCD]";
public static string LabelSound = "[ADS|Sound]";
public static bool AntennaOpen = false;
public static bool AntennaState = false;
public static bool DepthAlgorithm = true;
public static string LabelMissile = "[Missile 0]";
public static int GasTank = 50;
public static bool _fire = false;
public int _soundtick = -1;
private string FireControllerId = "";
public Missile(MyGridProgram program)
{
_program = program;
}
internal void initMissile()
{
if (IsMissile)
{
_program.Me.CustomName = LabelMissile;
_program.GridTerminalSystem.GetBlocksOfType<IMyShipController>(_shipControllers, block => block.CubeGrid.EntityId == _program.Me.CubeGrid.EntityId && block.CustomData.Contains(LabelMissile));
if (_shipControllers.Count() == 0)
{
_state = STATE.ERROR;
_program.Echo("运行导弹脚本....");
_program.Echo("运行指令实例:label=001在编程方块 表示导弹代号001");
_program.Echo("没有找到导弹远程控制方块");
return;
}
_cockpit = _shipControllers[0];
_program.GridTerminalSystem.GetBlocksOfType<IMyTerminalBlock>(terminalBlocks, block => block.CubeGrid.EntityId == _program.Me.CubeGrid.EntityId && block.CustomData.Contains(LabelMissile));
foreach (var t in terminalBlocks)
{
if (t is IMyShipMergeBlock)
{
_mergeBlocks.Add((IMyShipMergeBlock)t);
}
else if (t is IMyShipConnector)
{
_cnnBlocks.Add((IMyShipConnector)t);
}
else if (t is IMyGasTank)
{
_gasTanks.Add((IMyGasTank)t);
}
else if (t is IMyWarhead)
{
_warheadBlocks.Add((IMyWarhead)t);
}
else if (t is IMySensorBlock)
{
_sensorBlocks.Add((IMySensorBlock)t);
}
else if (t is IMyRadioAntenna)
{
((IMyRadioAntenna)t).Radius = 1f;
((IMyRadioAntenna)t).EnableBroadcasting = false;
((IMyRadioAntenna)t).Enabled = false;
((IMyRadioAntenna)t).HudText = "";
((IMyRadioAntenna)t).CustomName = "";
_radioBlocks.Add((IMyRadioAntenna)t);
}
}
foreach (var w in _warheadBlocks)
{
w.IsArmed = false;
w.DetonationTime = 150;
}
foreach (var s in _sensorBlocks)
{
s.Enabled = false;
s.DetectOwner = false;
s.DetectAsteroids = false;
s.DetectFriendly = false;
s.DetectPlayers = false;
s.DetectFloatingObjects = false;
}
SetChargeMode(ChargeMode.Recharge);
_state = STATE.RECHARGE;
_cameras = new MYCameras(_program, _cockpit, UpdateFrequency.Update1);
_cameras.InitMissileCameras();
}
else
{
_program.GridTerminalSystem.GetBlocksOfType<IMyShipController>(_shipControllers, block => block.CustomName.StartsWith("*"));
if (_shipControllers.Count() == 0)
{
_state = STATE.ERROR;
_program.Echo("运行火控脚本....");
_program.Echo("没有找到飞船的主控制方块");
_program.Echo("请在飞船的住控制方块前加 *");
return;
}
_program.GridTerminalSystem.GetBlocksOfType<IMyProjector>(_projector);
foreach (var p in _projector)
{
if (p.IsProjecting)
{
if (p.ProjectionOffset.Length() == 0 && p.ProjectionRotation.Length() == 0 && !string.IsNullOrEmpty(p.CustomData))
{
string[] SplitData = p.CustomData.Split('='); _program.Echo(SplitData.Length + "");
if (SplitData.Length == 4)
{
Vector3I ProjectionOffset, ProjectionRotation;
if (Vector3I.TryParseFromString(SplitData[1], out ProjectionOffset)) { p.ProjectionOffset = ProjectionOffset; }
if (Vector3I.TryParseFromString(SplitData[3], out ProjectionRotation)) { p.ProjectionRotation = ProjectionRotation; }
p.UpdateOffsetAndRotation();
}
}
else
{
string Offset = p.ProjectionOffset.X + ";" + p.ProjectionOffset.Y + ";" + p.ProjectionOffset.Z;
string Rotation = p.ProjectionRotation.X + ";" + p.ProjectionRotation.Y + ";" + p.ProjectionRotation.Z;
p.CustomData = "ProjectionOffset=" + Offset.Trim() + "=" + "ProjectionRotation=" + Rotation.Trim();
}
}
}
_cockpit = _shipControllers[0];
_program.GridTerminalSystem.GetBlocksOfType<IMySoundBlock>(_soundBlocks, block => block.CustomName.Contains(LabelSound));
SelectSounds("SoundBlockObjectiveComplete", 3);
_mylcd = new MYLCD(_program);
_cameras = new MYCameras(_program, _cockpit, UpdateFrequency.Update10);
_state = STATE.READY;
}
}
internal void Ready()
{
if (IsMissile)
{
_program.Runtime.UpdateFrequency = UpdateFrequency.None;
}
else
{
RenderLCD();
if (string.IsNullOrEmpty(RemoteKey)) { GetReadyMissiles(); }
_reqDictionary.Clear();
_cameras.InitFireControllerCameras();
_program.Runtime.UpdateFrequency = UpdateFrequency.None;
}
}
Vector3D MissilePositionPrev = new Vector3D();
Vector3D TargetPositionPrev = new Vector3D();
const double TICK_DPDATE1 = 0.0166666;
double PNGain = 3;
internal void AttackTarget(IMyIntergridCommunicationSystem IGC)
{
double _offset = _cameras.LockTarget._offset;
_program.Echo("" + _offset);
if (_AttackMode == AttackMode.Center && _cameras.LockTarget.Distance < 7500)
{
if (_offset > 90) { RequestAttackTarget(IGC); }
else { if (Missile.AntennaOpen != AntennaState) OpenRadioAntenna(Missile.AntennaOpen); _cameras.LockTheTarget(); }
}
Vector3D TargetVelocity = _cameras.LockTarget.Velocity;
Vector3D TargetPosition = _cameras.LockTarget.NextPosition(_AttackMode);
Vector3D targetAcceleration = _cameras.LockTarget.Acceleration;
Vector3D MissilePosition = _cockpit.CenterOfMass;
Vector3D MissileVelocity = _cockpit.GetShipVelocities().LinearVelocity;
Vector3D missileToTargetNormPrev = Vector3D.Normalize(TargetPositionPrev - MissilePositionPrev);
Vector3D missileToTargetNorm = Vector3D.Normalize(TargetPosition - MissilePosition);
double distance = Vector3D.Distance(MissilePosition, TargetPosition);
double[] ThrustsPowers = _thrusts.GetThrustsPowers(_NavMode == NavMode.Vector);
MatrixD refLookAtMatrix = MatrixD.CreateLookAt(new Vector3D(), _cockpit.WorldMatrix.Backward, _cockpit.WorldMatrix.Up);
Vector3D VelocityToMe = Vector3D.TransformNormal(_cockpit.GetShipVelocities().LinearVelocity, refLookAtMatrix);
Base6Directions.Direction direction = _cockpit.WorldMatrix.GetClosestDirection(missileToTargetNorm);
//--------------ProportionalGuidance---------------------------------
//比例导引的逻辑是，让导弹速度矢量的转动角速度（通俗的说，导弹转弯角速度）和导弹-目标视线（Line Of Sight，LOS）的旋转角速度成正比。这种导引方式能实现导弹前置截击。
Vector3D Rel_Vel = Vector3D.Normalize(TargetVelocity - MissileVelocity);
Vector3D am; double NavMult = 0.0; Vector3D LOS_Delta;
if (missileToTargetNormPrev.Length() != 0)
{ LOS_Delta = missileToTargetNorm - missileToTargetNormPrev; NavMult = LOS_Delta.Length() / TICK_DPDATE1; }
double relativeVelocity = (TargetVelocity - MissileVelocity).Length();
Vector3D LateralDirection = Vector3D.Normalize(Vector3D.Cross(Vector3D.Cross(Rel_Vel, missileToTargetNorm), Rel_Vel));
Vector3D lateralTargetAcceleration = (targetAcceleration - Vector3D.Dot(targetAcceleration, missileToTargetNorm) * missileToTargetNorm);
Vector3D LateralAccelerationComponent = LateralDirection * PNGain * NavMult * relativeVelocity + NavMult * 9.8 * 0.1 * PNGain;
double OversteerReqt = (LateralAccelerationComponent).Length() / ThrustsPowers[0];
if (OversteerReqt > 0.98)
{
LateralAccelerationComponent = ThrustsPowers[0] * Vector3D.Normalize(LateralAccelerationComponent + (OversteerReqt * Vector3D.Normalize(-MissileVelocity)) * LateralAccelerationComponent.Length() * 1 / 2);
}
double RejectedAccel = Math.Sqrt(ThrustsPowers[0] * ThrustsPowers[0] - LateralAccelerationComponent.LengthSquared());
if (double.IsNaN(RejectedAccel)) { RejectedAccel = 0; }
LateralAccelerationComponent = LateralAccelerationComponent + missileToTargetNorm * RejectedAccel + lateralTargetAcceleration;
if (_NavMode == NavMode.Standard)
{
am = Vector3D.Normalize(LateralAccelerationComponent);
Vector3D AimAtPosition = MissilePosition + am * 100;
_gyro.AimAtPosition(AimAtPosition);
_thrusts.SingleDirectionThrustControl(0, VelocityToMe.Z - FlightSpeed, ThrustsPowers[1], ThrustsPowers[0], MYThrusts.Direction.Backward, MYThrusts.Direction.Forward, FlightSpeed);
_thrusts.SingleDirectionThrustControl(0, VelocityToMe.X, ThrustsPowers[3], ThrustsPowers[2], MYThrusts.Direction.Right, MYThrusts.Direction.Left, FlightSpeed);
_thrusts.SingleDirectionThrustControl(0, VelocityToMe.Y, ThrustsPowers[5], ThrustsPowers[4], MYThrusts.Direction.Down, MYThrusts.Direction.Up, FlightSpeed);
}
else if (_NavMode == NavMode.Vector)
{
Vector3D GravityComp = -_cockpit.GetNaturalGravity();
am = Vector3D.Normalize(LateralAccelerationComponent + GravityComp);
Vector3D AimAtPosition = MissilePosition + am * 100;
_gyro.AimAtPosition(AimAtPosition);
if (GravityComp.Length() > 0.001)
_thrusts.SetThrustOverrideByDirection(MYThrusts.Direction.Forward, 1);
else
_thrusts.SingleDirectionThrustControl(0, VelocityToMe.Z - FlightSpeed, ThrustsPowers[1], ThrustsPowers[0], MYThrusts.Direction.Backward, MYThrusts.Direction.Forward, FlightSpeed);
}
TargetPositionPrev = TargetPosition;
MissilePositionPrev = MissilePosition;
if (distance < _cameras.LockTarget.BoundingBox.HalfExtents.Length() + 100)
{
if (!IsArmed)
{
foreach (var w in _warheadBlocks)
{ w.IsArmed = true; }
IsArmed = true;
}
if (distance <= 5)
{
foreach (var w in _warheadBlocks) { w.Detonate(); }
_program.Runtime.UpdateFrequency = UpdateFrequency.None;
}
}
}
internal void NavigationTo()
{
Vector3D TargetPosition = _cameras.LockTarget.HitPosition;
double[] ThrustsPowers = _thrusts.GetThrustsPowers(_NavMode == NavMode.Vector);
Vector3D NavigationPosition = _cockpit.CenterOfMass;
MatrixD refLookAtMatrix = MatrixD.CreateLookAt(new Vector3D(), _cockpit.WorldMatrix.Backward, _cockpit.WorldMatrix.Up);
double distance = Vector3D.Distance(TargetPosition, NavigationPosition);
Vector3D VelocityToMe = Vector3D.TransformNormal(_cockpit.GetShipVelocities().LinearVelocity, refLookAtMatrix);
Vector3D TargetToMe = Vector3D.TransformNormal(TargetPosition - _cockpit.CenterOfMass, refLookAtMatrix);
double elevation;
if (distance > VelocityToMe.Length() * 15)
{
if (_cockpit.TryGetPlanetElevation(MyPlanetElevation.Surface, out elevation))
{
Vector3D NaturalGravity = _cockpit.GetNaturalGravity().Normalized();
NavigationPosition = NavigationPosition - NaturalGravity * (FlightHeight - elevation);
if (_gyro.AimAtPosition(TargetPosition, _cockpit.CenterOfMass + NaturalGravity * 1000))
{
if (Math.Abs(TargetToMe.Z) < VelocityToMe.Length() * 15) { _state = STATE.ATTACK; }
}
}
else { _gyro.AimAtPosition(TargetPosition); }
Vector3D PosToMe = Vector3D.TransformNormal(NavigationPosition - _cockpit.CenterOfMass, refLookAtMatrix);
_thrusts.SingleDirectionThrustControl(0, VelocityToMe.Z - FlightSpeed, ThrustsPowers[1], ThrustsPowers[0], MYThrusts.Direction.Backward, MYThrusts.Direction.Forward, FlightSpeed);
_thrusts.SingleDirectionThrustControl(PosToMe.X, VelocityToMe.X, ThrustsPowers[3], ThrustsPowers[2], MYThrusts.Direction.Right, MYThrusts.Direction.Left, 0);
_thrusts.SingleDirectionThrustControl(PosToMe.Y, VelocityToMe.Y, ThrustsPowers[5], ThrustsPowers[4], MYThrusts.Direction.Down, MYThrusts.Direction.Up, FlightSpeed);
}
else { _state = STATE.ATTACK; }
}
internal void Raycast(IMyIntergridCommunicationSystem IGC)
{
if (_timetick % 2 != 0) return;
if (_cameras.RaycastLine())
{
SelectSounds("SoundBlockEnemyDetected", 3, 1);
_state = STATE.LOCKON; _fire = false;
_lastState = STATE.SEARCH;
RenderLCD();
}
}
internal void TurretSearch(IMyIntergridCommunicationSystem IGC)
{
if (_timetick % 2 != 0) return;
if (_cameras.SearchByTurret())
{
SelectSounds("SoundBlockEnemyDetected", 3, 1);
_state = STATE.LOCKON; _fire = true;
_lastState = STATE.TURRET;
_lastFireTick = _timetick - 53;
RenderLCD();
}
}
internal void KeepLockTarget(IMyIntergridCommunicationSystem IGC, IMyBroadcastListener broadcastListener)
{
if (_cameras.LockTheTarget())
{
if (_reqDictionary.Count() > 0)
{
string Target = BuilderTarget();
foreach (var m in _reqDictionary) { IGC.SendBroadcastMessage(m.Key, Target); }
broadcastListener.SetMessageCallback();
_reqDictionary.Clear();
}
if (_fire) Fire(IGC);
}
if (_cameras.LockTarget._offset <= 18)
{
if (_cameras.LockTarget._offset == 8 || _cameras.LockTarget._offset == 13 || _cameras.LockTarget._offset == 16) { SelectSounds("SoundBlockAlert1", 0.5f, 1); }
;
if (_timetick % 3 != 0) return;
RenderLCD();
}
else if (_lastState == STATE.SEARCH)
{
SelectSounds("SoundBlockAlert1", 0.5f, 1);
_state = STATE.READY;
}
else if (_lastState == STATE.TURRET)
{
SelectSounds("SoundBlockAlert2", 300);
_state = STATE.TURRET;
RenderLCD();
}
}
internal float GetTankFilledRatio()
{
float containerUsed = 0;
float containerTotal = 0;
for (int i = 0; i < _gasTanks.Count; i++)
{
if (_gasTanks[i].BlockDefinition.SubtypeName.Contains("HydrogenTank"))
{
containerUsed += _gasTanks[i].IsFunctional ? (float)(_gasTanks[i].Capacity * _gasTanks[i].FilledRatio) : 0;
containerTotal += (float)_gasTanks[i].Capacity;
_gasTanks[i].Enabled = true;
}
}
float usage = containerTotal == 0 ? 100 : containerUsed / containerTotal * 100;
return usage;
}
internal void SetChargeMode(ChargeMode chargeMode)
{
for (int i = 0; i < _gasTanks.Count; i++)
{
if (_gasTanks[i].BlockDefinition.SubtypeName.Contains("HydrogenTank"))
{
_gasTanks[i].Enabled = true;
_gasTanks[i].Stockpile = chargeMode == ChargeMode.Recharge;
}
}
}
internal void Recharge()
{
float h2 = GetTankFilledRatio();
foreach (var c in _cnnBlocks)
{
if (c.Status == MyShipConnectorStatus.Connectable)
{
c.Connect();
}
}
if (h2 >= GasTank)
{
SendReadyState();
}
else
{
_program.Echo("GasTank=" + h2.ToString("F2"));
}
}
internal void SendReadyState()
{
IMyTerminalBlock fireControl = _program.GridTerminalSystem.GetBlockWithName("*Fire Control");
if (fireControl != null && fireControl is IMyProgrammableBlock)
{
((IMyProgrammableBlock)fireControl).TryRun("ReadyMissile=" + LabelMissile + "=" + _program.Me.EntityId);
_state = STATE.READY;
}
}
internal void GetReadyMissiles()
{
_missileList.Clear();
List<IMyProgrammableBlock> _programmableBlocks = new List<IMyProgrammableBlock>();
_program.GridTerminalSystem.GetBlocksOfType<IMyProgrammableBlock>(_programmableBlocks, block => block.CustomName.ToLower().StartsWith("[missile"));
foreach (var p in _programmableBlocks) p.TryRun("Recharge");
RenderLCD();
}
internal void AddReadyMissile(string entity, IMyIntergridCommunicationSystem IGC)
{
string[] splitdata = entity.Split('=');
if (splitdata.Length == 3)
{
foreach (var m in _remoteFireController) IGC.SendBroadcastMessage(m.Key + "", "ReadyMissile=" + splitdata[2]);
if (!_missileList.ContainsKey(splitdata[2]))
{
_missileList.Add(splitdata[2], _timetick);
}
_missileList = _missileList.OrderBy(d => d.Value).ToDictionary(d => d.Key, d => d.Value);
RenderLCD();
}
}
internal void Fire(IMyIntergridCommunicationSystem IGC)
{
if (!IsMissile && _missileList.Count() > 0)
{
if (_state != STATE.LOCKON || _missileList.Count() == 0) return;
long entityId; long.TryParse(_missileList.FirstOrDefault().Key, out entityId);
if (string.IsNullOrEmpty(RemoteKey))
{
IMyProgrammableBlock PBlock = _program.GridTerminalSystem.GetBlockWithId(entityId) as IMyProgrammableBlock;
if (PBlock == null) return;
if (_lastState == STATE.TURRET && _timetick - _lastFireTick < 60) return;
if (PBlock.TryRun(BuilderTarget()))
{
_missileList.Remove(entityId + "");
foreach (var m in _remoteFireController) IGC.SendBroadcastMessage(m.Key + "", "RemoveMissile=" + entityId);
_fire = _lastState == STATE.TURRET ? true : false;
_lastFireTick = _timetick;
}
else { GetReadyMissiles(); }
}
else
{
string Target = "RemoteFire" + BuilderTarget();
IGC.SendBroadcastMessage(RemoteKey, Target);
_fire = false;
}
}
}
internal void RemoteFire(IMyIntergridCommunicationSystem IGC, string Target)
{
if (!IsMissile && _missileList.Count() > 0)
{
string[] splitdata = Target.Split('=');
string target = "";
for (int i = 0; i < splitdata.Length; i++)
{
if (i == 8)
{
target += Missile.DetachDistance + "=";
}
else if (i < splitdata.Length - 1)
{
target += splitdata[i] + "=";
}
else { target += splitdata[i]; }
}
long entityId; long.TryParse(_missileList.FirstOrDefault().Key, out entityId);
IMyProgrammableBlock PBlock = _program.GridTerminalSystem.GetBlockWithId(entityId) as IMyProgrammableBlock;
if (PBlock == null) return;
if (_lastState == STATE.TURRET && _timetick - _lastFireTick < 60) return;
if (PBlock.TryRun(target))
{
_missileList.Remove(entityId + "");
foreach (var m in _remoteFireController) IGC.SendBroadcastMessage(m.Key + "", "RemoveMissile=" + entityId);
_lastFireTick = _timetick;
}
}
}
internal void SplitGPSInfo(IMyIntergridCommunicationSystem IGC, string argument)
{
if (IsMissile) return;
try
{
String[] splitData = argument.Split(':');
if (splitData.Length == 7)
{
double x = Double.Parse(splitData[2]); double y = Double.Parse(splitData[3]); double z = Double.Parse(splitData[4]);
LockTarget lockTarget = new LockTarget(UpdateFrequency.Update10);
lockTarget.HitPosition = new Vector3D(x, y, z);
lockTarget.Position = new Vector3D(x, y, z);
lockTarget.EntityId = -1;
lockTarget.ScanTimeTick = 0;
StringBuilder TargetInfo = new StringBuilder();
TargetInfo.Append("Target").Append("=");
TargetInfo.Append(lockTarget.EntityId).Append("=");
TargetInfo.Append(lockTarget.Position).Append("=");
TargetInfo.Append(lockTarget.Velocity).Append("=");
TargetInfo.Append(lockTarget.Acceleration).Append("=");
TargetInfo.Append(lockTarget.HitPosition).Append("=");
TargetInfo.Append(_timetick - lockTarget.ScanTimeTick).Append("=");
TargetInfo.Append(Missile.FlightSpeed).Append("=");
TargetInfo.Append(Missile.DetachDistance).Append("=");
TargetInfo.Append(Missile.AntennaOpen).Append("=");
TargetInfo.Append(AttackMode.AimPosition).Append("=");
TargetInfo.Append(_program.Me.EntityId + "");
string target = TargetInfo.ToString();
long entityId; long.TryParse(_missileList.FirstOrDefault().Key, out entityId);
IMyProgrammableBlock PBlock = _program.GridTerminalSystem.GetBlockWithId(entityId) as IMyProgrammableBlock;
if (PBlock == null) return;
if (_lastState == STATE.TURRET && _timetick - _lastFireTick < 60) return;
if (PBlock.TryRun(target))
{
_missileList.Remove(entityId + "");
foreach (var m in _remoteFireController) IGC.SendBroadcastMessage(m.Key + "", "RemoveMissile=" + entityId);
_lastFireTick = _timetick;
}
}
}
catch { }
}
internal string BuilderTarget()
{
var lockTarget = _cameras.LockTarget;
StringBuilder TargetInfo = new StringBuilder();
TargetInfo.Append("Target").Append("=");
TargetInfo.Append(lockTarget.EntityId).Append("=");
TargetInfo.Append(lockTarget.Position).Append("=");
TargetInfo.Append(lockTarget.Velocity).Append("=");
TargetInfo.Append(lockTarget.Acceleration).Append("=");
TargetInfo.Append(_AttackMode == AttackMode.AimPosition ? lockTarget.FirstHitPosition : lockTarget.HitPosition).Append("=");
TargetInfo.Append(_timetick - lockTarget.ScanTimeTick).Append("=");
if (lockTarget.Type == MyDetectedEntityType.SmallGrid && FlightSpeed > 650)
TargetInfo.Append("650").Append("=");
else
TargetInfo.Append(Missile.FlightSpeed).Append("=");
TargetInfo.Append(Missile.DetachDistance).Append("=");
TargetInfo.Append(Missile.AntennaOpen).Append("=");
TargetInfo.Append(Missile._AttackMode).Append("=");
TargetInfo.Append(_program.Me.EntityId + "");
return TargetInfo.ToString();
}
internal long AnalysisTarget(string target, UpdateType updateType)
{
string[] splitdata = target.Split('=');
if (splitdata.Length == 12)
{
int offset = 0;
if (!long.TryParse(splitdata[1], out _cameras.LockTarget.EntityId)) return 0;
if (!Vector3D.TryParse(splitdata[2], out _cameras.LockTarget.Position)) return 0;
if (!Vector3D.TryParse(splitdata[3], out _cameras.LockTarget.Velocity)) return 0;
if (!Vector3D.TryParse(splitdata[4], out _cameras.LockTarget.Acceleration)) return 0;
if (!Vector3D.TryParse(splitdata[5], out _cameras.LockTarget.HitPosition)) return 0;
if (!int.TryParse(splitdata[6], out offset)) return 0;
if (!double.TryParse(splitdata[7], out Missile.FlightSpeed)) return 0;
if (!double.TryParse(splitdata[8], out Missile.DetachDistance)) return 0;
if (!bool.TryParse(splitdata[9], out Missile.AntennaOpen)) return 0;
_AttackMode = (AttackMode)Enum.Parse(typeof(AttackMode), splitdata[10]);
FireControllerId = splitdata[11];
_cameras.LockTarget.Deviation = _cameras.LockTarget.HitPosition - _cameras.LockTarget.Position;
if (_AttackMode == AttackMode.AimPosition) { _cameras.LockTarget.FirstHitPosition = _cameras.LockTarget.HitPosition; }
_cameras.LockTarget.ScanTimeTick = Missile._timetick - offset * 10;
_cameras.LockTarget.NumberFail = 1;
if (updateType == UpdateType.Terminal)
{
foreach (var c in _cnnBlocks) { c.PullStrength = 0f; c.Disconnect(); }
foreach (var m in _mergeBlocks) { m.Enabled = false; }
}
return _cameras.LockTarget.EntityId;
}
return 0;
}
internal void CheakMissile()
{
foreach (var m in _mergeBlocks)
{
if (m.IsConnected) { m.Enabled = false; return; }
}
_program.GridTerminalSystem.GetBlocksOfType<IMyShipController>(_shipControllers, block => block.CubeGrid.EntityId == _program.Me.CubeGrid.EntityId && (block.CustomData.Contains(LabelMissile) || block.CustomName.StartsWith("*")));
if (_shipControllers.Count() == 0) { _state = STATE.ERROR; _program.Echo("No Found Ship Controller"); return; }
_cockpit = _shipControllers[0];
LaunchVelocity = _cockpit.GetShipVelocities().LinearVelocity;
DetachDistance = Math.Max(LaunchVelocity.Length() + DetachDistance, DetachDistance);
DetachPosition = _cockpit.CenterOfMass + _cockpit.WorldMatrix.Forward * DetachDistance;
SetChargeMode(ChargeMode.Auto);
_gyro = new MYGyro(_program, _cockpit);
_thrusts = new MYThrusts(_program, _cockpit);
_NavMode = _thrusts.InitMYThrusts() ? NavMode.Standard : NavMode.Vector;
_gyro.InitGyroscope();
_cameras.InitMissileCameras();
_state = STATE.DETACH;
return;
}
internal void Detachment()
{
_cockpit.DampenersOverride = false;
double[] ThrustsPowers = _thrusts.GetThrustsPowers();
MatrixD refLookAtMatrix = MatrixD.CreateLookAt(new Vector3D(), _cockpit.WorldMatrix.Backward, _cockpit.WorldMatrix.Up);
Vector3D TargetPToMe = Vector3D.TransformNormal(DetachPosition - _cockpit.CenterOfMass, refLookAtMatrix);
Vector3D VelocityToMe = Vector3D.TransformNormal(_cockpit.GetShipVelocities().LinearVelocity, refLookAtMatrix);
Vector3D LaunchVToMe = Vector3D.TransformNormal(LaunchVelocity, refLookAtMatrix);
if (_NavMode == NavMode.Vector && _cockpit.GetNaturalGravity().Length() > 2.0)
{
Vector3D NaturalGravity = _cockpit.GetNaturalGravity().Normalized();
if (_cockpit.WorldMatrix.GetClosestDirection(NaturalGravity) != Base6Directions.Direction.Backward)
{
double elevation;
if (_cockpit.TryGetPlanetElevation(MyPlanetElevation.Surface, out elevation))
{
if (elevation < DetachDistance)
{
Vector3D AimAtPosition = _cockpit.CenterOfMass + NaturalGravity * ThrustsPowers[0] + _cockpit.WorldMatrix.Forward * _cockpit.GetNaturalGravity().Length();
_gyro.AimAtPosition(DetachPosition, AimAtPosition);
}
}
}
}
if (_NavMode == NavMode.Standard)
{
_thrusts.SingleDirectionThrustControl(0, VelocityToMe.X - LaunchVToMe.X, ThrustsPowers[3], ThrustsPowers[2], MYThrusts.Direction.Right, MYThrusts.Direction.Left, FlightSpeed);
_thrusts.SingleDirectionThrustControl(0, VelocityToMe.Y - LaunchVToMe.Y, ThrustsPowers[5], ThrustsPowers[4], MYThrusts.Direction.Down, MYThrusts.Direction.Up, FlightSpeed);
}
_thrusts.SingleDirectionThrustControl(TargetPToMe.Z, VelocityToMe.Z - LaunchVToMe.Z, ThrustsPowers[0], ThrustsPowers[0], MYThrusts.Direction.Backward, MYThrusts.Direction.Forward, FlightSpeed);
if (Math.Abs(TargetPToMe.Z) < 3)
{
foreach (var w in _warheadBlocks) { w.DetonationTime = _AttackMode == AttackMode.AimPosition ? 300 : 150; w.StartCountdown(); }
foreach (var s in _sensorBlocks) s.Enabled = true;
_state = _AttackMode == AttackMode.AimPosition && _NavMode == NavMode.Standard ? STATE.NAVIGATION : STATE.ATTACK;
FlightHeight = 5 * FlightSpeed;
FlightHeight = Math.Max(500, FlightHeight);
_cameras.LockTheTarget();
}
if (Math.Abs(TargetPToMe.Z) < DetachDistance / 2) _cameras.LockTheTarget();
}
internal void RequestAttackTarget(IMyIntergridCommunicationSystem IGC)
{
if (_messageTick != -1 && _timetick - _messageTick > 90)
{
_messageTick = _timetick;
OpenRadioAntenna(true);
IGC.SendBroadcastMessage(FireControllerId, "Request=" + _program.Me.EntityId);
}
}
internal void OpenRadioAntenna(bool open)
{
foreach (var r in _radioBlocks)
{
if (r.Closed) continue;
r.Radius = open ? 15000 : 1f;
r.EnableBroadcasting = true;
r.Enabled = true;
r.HudText = "";
}
AntennaState = open;
}
internal void SelectSounds(string soundName, float LoopPeriod, int tick = 1)
{
foreach (var s in _soundBlocks)
{
s.LoopPeriod = LoopPeriod;
s.SelectedSound = soundName;
}
if (_timetick > _soundtick) { _soundtick = _timetick + tick; }
}
internal void PlaySound()
{
if (_soundtick == _timetick) { foreach (var s in _soundBlocks) { s.Play(); } }
_soundtick = -1;
}
internal void RenderLCD() { _mylcd.Rendering(_state == STATE.LOCKON ? _cameras.LockTarget : null, _missileList.Count(), string.IsNullOrEmpty(RemoteKey)); }
public static void ChangeSpeed(IMyProgrammableBlock me, string argument)
{
if (argument.Contains("speed+") && FlightSpeed < 1000)
{
FlightSpeed += 50;
RsetConfig(me);
}
else if (argument.Equals("speed-") && FlightSpeed > 50)
{
FlightSpeed -= 50;
RsetConfig(me);
}
}
public static bool SetConfig(IMyProgrammableBlock me, string[] splitData)
{
bool.TryParse(splitData[0].Split('=')[1], out IsMissile);
MissilePwd = splitData[3].Split('=')[1];
double.TryParse(splitData[4].Split('=')[1], out FlightSpeed);
double.TryParse(splitData[5].Split('=')[1], out DetachDistance);
bool.TryParse(splitData[6].Split('=')[1].Trim(), out AntennaOpen);
LabelLCD = splitData[7].Split('=')[1].Trim();
LabelSound = splitData[8].Split('=')[1].Trim();
LabelMissile = splitData[11].Split('=')[1].Trim();
int.TryParse(splitData[12].Split('=')[1], out GasTank);
VERCION_CODE = splitData[14];
if (!IsMissile && !MissilePwd.Equals(me.EntityId + "")) RsetConfig(me);
return IsMissile;
}
public static void RsetConfig(IMyProgrammableBlock me)
{
StringBuilder stringBuilder = new StringBuilder();
stringBuilder.Append("是否是导弹(是-true,否-false)=" + IsMissile).AppendLine();
stringBuilder.AppendLine();
stringBuilder.Append("火控雷达的设置步骤，第一个是火控雷达的远程密钥(RemoteKey)自动生成").AppendLine();
stringBuilder.Append("RemoteKey=" + (!IsMissile ? me.EntityId + "" : "")).AppendLine();
stringBuilder.Append("最大飞行速度=" + FlightSpeed).AppendLine();
stringBuilder.Append("导弹脱离距离=" + DetachDistance).AppendLine();
stringBuilder.Append("是否开启天线=" + AntennaOpen).AppendLine();
stringBuilder.Append("屏幕显示标签=" + LabelLCD).AppendLine();
stringBuilder.Append("声音显示标签=" + LabelSound).AppendLine();
stringBuilder.AppendLine();
stringBuilder.Append("导弹设置内容，第一个(label)不要修改，使用指令：label=xxx设置").AppendLine();
stringBuilder.Append("Label=" + LabelMissile).AppendLine();
stringBuilder.Append("氢气百分比=" + GasTank).AppendLine();
stringBuilder.AppendLine();
stringBuilder.Append(VERCION_CODE);
me.CustomData = stringBuilder.ToString();
}
public static void SetAllLabel(Program _program, string label)
{
if (!Missile.IsMissile) return;
_program.Echo(label);
LabelMissile = "[Missile" + label.Replace("label=", " ") + "]";
List<IMyShipMergeBlock> _mergeBlocks = new List<IMyShipMergeBlock>();
_program.GridTerminalSystem.GetBlocksOfType<IMyShipMergeBlock>(_mergeBlocks, block => block.CubeGrid == _program.Me.CubeGrid);
foreach (var m in _mergeBlocks)
{
if (m.IsConnected)
{
_program.Echo("检测到导弹合并处于连接状态，请先断开合并方块，使导弹必须属于子网格!");
return;
}
}
_program.Me.CustomName = LabelMissile;
List<IMyTerminalBlock> terminalBlocks = new List<IMyTerminalBlock>();
_program.GridTerminalSystem.GetBlocksOfType<IMyTerminalBlock>(terminalBlocks, block => block.CubeGrid == _program.Me.CubeGrid);
foreach (var f in terminalBlocks)
{
if (!(f is IMyProgrammableBlock))
{
f.ShowInToolbarConfig = false;
f.ShowInTerminal = false;
f.CustomData = LabelMissile;
}
}
foreach (var m in _mergeBlocks)
{
m.Enabled = true;
}
RsetConfig(_program.Me);
}
}
#endregion
public class MYCameras
{
private IMyShipController _cockpit;
private MyGridProgram _program;
public List<IMyCameraBlock> _CameraBlocks = new List<IMyCameraBlock>();
private MyDetectedEntityInfo _entityInfo = new MyDetectedEntityInfo();
private List<IMyLargeTurretBase> _turretBlock = new List<IMyLargeTurretBase>();
private List<IMyTurretControlBlock> _turretControlBlock = new List<IMyTurretControlBlock>();
public LockTarget LockTarget;
public IMyCameraBlock Finder;
public IMyCameraBlock ActiveCamera;
private int MaxLocked = 30000;
public MYCameras(MyGridProgram program, IMyShipController cockpit, UpdateFrequency update)
{
_program = program;
_cockpit = cockpit;
LockTarget = new LockTarget(update);
}
public void InitMissileCameras()
{
ActiveCamera = null;
_program.GridTerminalSystem.GetBlocksOfType<IMyCameraBlock>(_CameraBlocks, block => block.CubeGrid == _program.Me.CubeGrid);
_program.GridTerminalSystem.GetBlocksOfType<IMyLargeTurretBase>(_turretBlock);
_program.GridTerminalSystem.GetBlocksOfType<IMyTurretControlBlock>(_turretControlBlock);
foreach (var camrea in _CameraBlocks)
{
if (camrea.IsFunctional) { camrea.EnableRaycast = true; }
if (camrea.IsActive) { ActiveCamera = camrea; }
}
}
public void InitFireControllerCameras(int MaxLocked = 30000)
{
ActiveCamera = null;
this.MaxLocked = MaxLocked;
_program.GridTerminalSystem.GetBlocksOfType<IMyCameraBlock>(_CameraBlocks, block => !block.CustomData.ToLower().StartsWith("[missile"));
_program.GridTerminalSystem.GetBlocksOfType<IMyLargeTurretBase>(_turretBlock);
_program.GridTerminalSystem.GetBlocksOfType<IMyTurretControlBlock>(_turretControlBlock);
foreach (var camrea in _CameraBlocks)
{
if (camrea.IsFunctional) { camrea.EnableRaycast = true; }
if (camrea.IsActive) { ActiveCamera = camrea; }
}
}
internal bool SearchByTurret()
{
_entityInfo = new MyDetectedEntityInfo();
foreach (var turret in _turretControlBlock)
{
if (turret.IsAimed)
{
if (turret.GetTargetedEntity().IsEmpty()) continue;
if (_entityInfo.BoundingBox.Size.Length() < turret.GetTargetedEntity().BoundingBox.Size.Length())
{
_entityInfo = turret.GetTargetedEntity();
}
}
}
foreach (var turret in _turretBlock)
{
if (turret.IsAimed)
{
if (turret.GetTargetedEntity().IsEmpty()) continue;
if (_entityInfo.BoundingBox.Size.Length() < turret.GetTargetedEntity().BoundingBox.Size.Length())
{ _entityInfo = turret.GetTargetedEntity(); }
}
}
if (_entityInfo.BoundingBox.Size.Length() > 15)
{
LockTarget.Update(_entityInfo, _cockpit.CenterOfMass, false);
Missile._AttackMode = _entityInfo.Velocity.Length() == 0 ? AttackMode.AimPosition : AttackMode.Center;
return true;
}
return false;
}
internal bool RaycastLine()
{
Vector3D position = ActiveCamera == null ? _cockpit.GetPosition() + _cockpit.WorldMatrix.Forward * 15000 : ActiveCamera.GetPosition() + ActiveCamera.WorldMatrix.Forward * 15000;
foreach (var camrea in _CameraBlocks)
{
if (camrea.IsActive) continue;
if (!camrea.EnableRaycast) camrea.EnableRaycast = true;
if (!camrea.CanScan(position)) continue;
_entityInfo = camrea.Raycast(position);
if (_entityInfo.IsEmpty()) break;
if (_entityInfo.Relationship != MyRelationsBetweenPlayerAndBlock.Owner)
{
if (_entityInfo.Type == MyDetectedEntityType.LargeGrid ||
_entityInfo.Type == MyDetectedEntityType.SmallGrid ||
_entityInfo.Type == MyDetectedEntityType.CharacterHuman)
{
if (Vector3D.Distance(_entityInfo.Position, camrea.GetPosition()) >= 50)
{
LockTarget.Update(_entityInfo, _cockpit.CenterOfMass, false);
Missile._AttackMode = _entityInfo.Velocity.Length() == 0 ? AttackMode.AimPosition : AttackMode.Center;
return true;
}
}
}
break;
}
return false;
}
internal bool LockTheTarget()
{
Vector3D[] currents = LockTarget.LockPosition(_cockpit.CenterOfMass, true);
if (currents == null) return false;
int i = 0; int j = 0;
foreach (var camrea in _CameraBlocks)
{
if (!camrea.IsFunctional) continue;
if (!camrea.EnableRaycast) camrea.EnableRaycast = true;
if (!camrea.CanScan(currents[i])) continue;
_entityInfo = camrea.Raycast(currents[i]);
if (!_entityInfo.IsEmpty())
{
if (_entityInfo.BoundingBox.Size.Length() <= 15) continue;
if (Vector3D.Distance(_entityInfo.Position, camrea.GetPosition()) > MaxLocked) continue;
if (_entityInfo.EntityId == LockTarget.EntityId)
{
LockTarget.Update(_entityInfo, _cockpit.CenterOfMass, true);
return true;
}
}
i++; j++;
if (i >= currents.Length)
{
LockTarget.NumberFail++;
currents = LockTarget.LockPosition(_cockpit.CenterOfMass, false);
if (currents == null || j > 23) return false;
i = 0;
}
}
return false;
}
}
public class LockTarget
{
public long EntityId; public string Name; public MyDetectedEntityType Type = MyDetectedEntityType.SmallGrid; public MyRelationsBetweenPlayerAndBlock Relationship;
public Vector3D HitPosition; public Vector3D FirstHitPosition; public MatrixD Orientation; public Vector3D Velocity; public BoundingBoxD BoundingBox; public Vector3D Position;
public int ScanTimeTick; public int _offset; public Vector3D Acceleration; public Vector3D Deviation; public double Distance;
public int NumberFail = 1; private Vector3D[] vList = new Vector3D[4]; const double TICK_DPDATE10 = 0.1666660; const double TICK_DPDATE1 = 0.0166666; UpdateFrequency _update; double ScanInterval = TICK_DPDATE10;
public LockTarget(UpdateFrequency update) { _update = update; }
public void Update(MyDetectedEntityInfo entityInfo, Vector3D CenterOfMass, bool isLock)
{
if (isLock)
{
_offset = Missile._timetick - ScanTimeTick;
Acceleration = (entityInfo.Velocity - Velocity) / (_offset * (_update == UpdateFrequency.Update1 ? TICK_DPDATE1 : TICK_DPDATE10));
}
else { NumberFail = 1; Acceleration = Vector3D.Zero; }
ScanTimeTick = Missile._timetick;
Distance = Vector3D.Distance(CenterOfMass, Position);
if (_update == UpdateFrequency.Update1) { if (Distance > 7500) { ScanInterval = 10000; } else if (Distance > 1600) { ScanInterval = TICK_DPDATE10; } else { ScanInterval = 3 * TICK_DPDATE1; } }
EntityId = entityInfo.EntityId; Name = entityInfo.Name; Type = entityInfo.Type; Orientation = entityInfo.Orientation;
Relationship = entityInfo.Relationship; Position = entityInfo.Position; Velocity = entityInfo.Velocity; BoundingBox = entityInfo.BoundingBox;
if (Vector3D.TryParse(entityInfo.HitPosition.ToString(), out HitPosition)) { if (!isLock) FirstHitPosition = HitPosition; Deviation = HitPosition - Position; } else { Deviation = Vector3D.Zero; HitPosition = Vector3D.Zero; }
//if (Velocity.Length() == 0 && Acceleration.Length() == 0 && HitPosition != Vector3D.Zero) { Position = HitPosition; Deviation = Vector3D.Zero; }
}
public static long NowTimeStamp()
{
TimeSpan ts = DateTime.Now - new DateTime(1970, 1, 1, 0, 0, 0, 0);
return Convert.ToInt64(ts.TotalMilliseconds);
}
internal Vector3D NextPosition(AttackMode mode)
{
if (mode == AttackMode.AimPosition)
return FirstHitPosition;
else
{
_offset = Missile._timetick - ScanTimeTick;
double time = _offset * TICK_DPDATE1;
return Position + Velocity * time + 0.5 * Acceleration * time * time;
}
}
internal Vector3D[] LockPosition(Vector3D CenterOfMass, bool isFirst)
{
_offset = Missile._timetick - ScanTimeTick;
double time = _offset * (_update == UpdateFrequency.Update1 ? TICK_DPDATE1 : TICK_DPDATE10);
if (time < ScanInterval) return null;
double Size = 2.5;
Vector3D movePosition1 = Velocity * time + 0.5 * Acceleration * time * time;
if (Size * NumberFail > BoundingBox.HalfExtents.Length())
{
NumberFail = 1;
if (!isFirst) { return null; }
}
vList[0] = Position + movePosition1 + Deviation;
vList[1] = Position + movePosition1;
Base6Directions.Direction direction = Orientation.GetClosestDirection(CenterOfMass - movePosition1);
if (direction == Base6Directions.Direction.Forward || direction == Base6Directions.Direction.Backward)
{
vList[2] = Position + movePosition1 + Orientation.Left * NumberFail * Size;
vList[3] = Position + movePosition1 + Orientation.Right * NumberFail * Size;
}
else
{
vList[2] = Position + movePosition1 + Orientation.Forward * NumberFail * Size;
vList[3] = Position + movePosition1 + Orientation.Backward * NumberFail * Size;
}
return vList;
}
}
public class MYLCD
{
private MyGridProgram _program;
private List<IMyTextPanel> textSurfaces = new List<IMyTextPanel>();
private VRageMath.Color color = Color.Gray;
private Color GREEN = new Color(0, 255, 0, 255);
private Color GRAY = new Color(204, 204, 204, 255);
public MYLCD(MyGridProgram program)
{
program.GridTerminalSystem.GetBlocksOfType<IMyTextPanel>(textSurfaces, block => block.CustomName.Contains(Missile.LabelLCD));
foreach (var t in textSurfaces)
{
t.ScriptBackgroundColor = Color.Black;
t.ContentType = ContentType.SCRIPT;
t.Script = "";
}
_program = program;
}
public void Rendering(LockTarget lockTarget, int missileCount, bool RemmotMissile)
{
if (textSurfaces.Count != 0)
{
color = lockTarget != null ? GREEN : GRAY;
StringBuilder targetInfo = new StringBuilder();
targetInfo.Append("D:" + (lockTarget != null ? (int)lockTarget.Distance + "m" : "-")).AppendLine();
targetInfo.Append("V:" + (lockTarget != null ? lockTarget.Velocity.Length().ToString("F2") + "m/s" : "-")).AppendLine();
targetInfo.Append("A:" + (lockTarget != null ? lockTarget.Acceleration.Length().ToString("F2") + "m/s" : "-")).AppendLine();
targetInfo.Append("S:" + (lockTarget != null ? (int)lockTarget.BoundingBox.Size.Length() + "m" : "-")).AppendLine();
targetInfo.Append("T:" + (lockTarget != null ? (lockTarget._offset / 6.0).ToString("F3") + "s" : "-"));
foreach (var t in textSurfaces)
{
MySpriteDrawFrame frame = t.DrawFrame();
Vector2 centerPos = t.TextureSize * 0.5f;
DrawTargetInfo(frame, new Vector2(centerPos.X + 15 - 256f, centerPos.X - 80), targetInfo.ToString(), 0.8f);
DrawSprites(frame, centerPos, lockTarget == null ? "Range:15000" : lockTarget.Name, RemmotMissile);
if (missileCount > 10) { missileCount = 10; }
for (var i = 0; i < missileCount; i++)
{
if (i == 0) { DrawRocket(frame, new Vector2(centerPos.X, centerPos.X + 256f - 40f), 0.2f); continue; }
var n = (int)((i - 1) / 2) + 1; var p = i % 2 == 0 ? -1 : 1;
DrawRocket(frame, new Vector2(centerPos.X + p * n * 50f, centerPos.X + 256f - 40f), 0.2f);
}
frame.Dispose();
}
}
}
public void DrawSprites(MySpriteDrawFrame frame, VRageMath.Vector2 centerPos, string TargetName, bool RemmotMissile = false, float scale = 1f, float rotation = 0f)
{
float sin = (float)Math.Sin(rotation);
float cos = (float)Math.Cos(rotation);
string data = RemmotMissile ? "MissileSpeed:" + Missile.FlightSpeed + " Offset:" + Missile._AttackMode
: "MissileSpeed:" + Missile.FlightSpeed + " MissileType:RemoteLaunch" + " Offset:" + Missile._AttackMode;
;
frame.Add(new MySprite() { Type = SpriteType.TEXTURE, Alignment = TextAlignment.CENTER, Data = "Grid", Position = centerPos, Size = new Vector2(512f, 512f), Color = new Color(0, 255, 255, 0.02f), RotationOrScale = rotation }); // background
frame.Add(new MySprite() { Type = SpriteType.TEXTURE, Alignment = TextAlignment.CENTER, Data = "DecorativeBracketLeft", Position = new Vector2(cos * -80f, sin * -80f) * scale + centerPos, Size = new Vector2(20f, 80f) * scale, Color = color, RotationOrScale = rotation }); // DecorativeBracketLeft
frame.Add(new MySprite() { Type = SpriteType.TEXTURE, Alignment = TextAlignment.CENTER, Data = "DecorativeBracketRight", Position = new Vector2(cos * 80f, sin * 80f) * scale + centerPos, Size = new Vector2(20f, 80f) * scale, Color = color, RotationOrScale = rotation }); // DecorativeBracketRight
frame.Add(new MySprite(SpriteType.TEXTURE, "AH_TextBox", new Vector2(-sin * -211f, cos * -211f) * scale + centerPos, new Vector2(200f, 60f) * scale, color, null, TextAlignment.CENTER, rotation)); // spriteState
frame.Add(new MySprite(SpriteType.TEXT, Missile._state + "", new Vector2(-sin * -223f, cos * -228f) * scale + centerPos, null, color, "Debug", TextAlignment.CENTER, 1f * scale)); // textState
frame.Add(new MySprite(SpriteType.TEXT, TargetName, new Vector2(-sin * -176f, cos * -170f) * scale + centerPos, null, color, "Debug", TextAlignment.CENTER, 1f * scale)); // textTargettName
frame.Add(new MySprite(SpriteType.TEXT, data, new Vector2(-sin * 0f, cos * 155f) * scale + centerPos, null, color, "Debug", TextAlignment.CENTER, 1f * 0.5f)); // textState
}
public void DrawTargetInfo(MySpriteDrawFrame frame, Vector2 centerPos, string data, float scale = 1f, float rotation = 0f, float colorScale = 1f)
{
float sin = (float)Math.Sin(rotation);
float cos = (float)Math.Cos(rotation);
frame.Add(new MySprite(SpriteType.TEXT, data, new Vector2(0f, 0f) * scale + centerPos, null, color, "Debug", TextAlignment.LEFT, 1f * scale)); // textD
}
public void DrawRocket(MySpriteDrawFrame frame, Vector2 centerPos, float scale = 1f, float rotation = 0f, float colorScale = 1f)
{
float sin = (float)Math.Sin(rotation);
float cos = (float)Math.Cos(rotation);
frame.Add(new MySprite(SpriteType.TEXTURE, "Triangle", new Vector2(-sin * -72f, cos * -72f) * scale + centerPos, new Vector2(96f, 96f) * scale, new Color(0f * colorScale, 1f * colorScale, 0f * colorScale, 1f), null, TextAlignment.CENTER, rotation)); // topFin
frame.Add(new MySprite(SpriteType.TEXTURE, "Triangle", new Vector2(-sin * 48f, cos * 48f) * scale + centerPos, new Vector2(96f, 96f) * scale, new Color(0f * colorScale, 1f * colorScale, 0f * colorScale, 1f), null, TextAlignment.CENTER, rotation)); // bottomFin
frame.Add(new MySprite(SpriteType.TEXTURE, "Circle", new Vector2(-sin * -96f, cos * -96f) * scale + centerPos, new Vector2(48f, 96f) * scale, new Color(0f * colorScale, 1f * colorScale, 0f * colorScale, 1f), null, TextAlignment.CENTER, rotation)); // noseCone
frame.Add(new MySprite(SpriteType.TEXTURE, "SquareSimple", new Vector2(-sin * 120f, cos * 120f) * scale + centerPos, new Vector2(96f, 48f) * scale, new Color(0f * colorScale, 1f * colorScale, 0f * colorScale, 1f), null, TextAlignment.CENTER, rotation)); // bottomFinBase
frame.Add(new MySprite(SpriteType.TEXTURE, "SquareSimple", new Vector2(0f, 0f) * scale + centerPos, new Vector2(48f, 192f) * scale, new Color(0f * colorScale, 1f * colorScale, 0f * colorScale, 1f), null, TextAlignment.CENTER, rotation)); // tube
frame.Add(new MySprite(SpriteType.TEXTURE, "SquareSimple", new Vector2(cos * -28f, sin * -28f) * scale + centerPos, new Vector2(10f, 288f) * scale, new Color(0f * colorScale, 0f * colorScale, 0f * colorScale, 1f), null, TextAlignment.CENTER, rotation)); // shadowLeft
frame.Add(new MySprite(SpriteType.TEXTURE, "SquareSimple", new Vector2(cos * 28f, sin * 28f) * scale + centerPos, new Vector2(10f, 288f) * scale, new Color(0f * colorScale, 0f * colorScale, 0f * colorScale, 1f), null, TextAlignment.CENTER, rotation)); // shadowRight
}
}
#region MYGyro
public class MYGyro
{
private IMyShipController _cockpit;
private MyGridProgram _program;
private List<IMyGyro> Gyroscopes = new List<IMyGyro>();
private PIDGyro PIDControllerYaw = new PIDGyro();
private PIDGyro PIDControllerPitch = new PIDGyro();
private PIDGyro PIDControllerRoll = new PIDGyro();
public MYGyro(MyGridProgram program, IMyShipController cockpit)
{
_program = program;
_cockpit = cockpit;
}
public void InitGyroscope()
{
_program.GridTerminalSystem.GetBlocksOfType<IMyGyro>(Gyroscopes, block => block.CubeGrid.EntityId == _program.Me.CubeGrid.EntityId);
foreach (IMyGyro g in Gyroscopes)
{
g.Enabled = true;
g.GyroOverride = false;
}
}
public bool AimAtPosition(Vector3D TargetPos, Vector3D Vertical = new Vector3D())
{
double YawValue = 0, PitchValue = 0, RollValue = 0;
if (TargetPos != Vector3D.Zero)
{
MatrixD refLookAtMatrix = MatrixD.CreateLookAt(new Vector3D(), _cockpit.WorldMatrix.Forward, _cockpit.WorldMatrix.Up);
Vector3D PositionToMe = Vector3D.Normalize(Vector3D.TransformNormal(TargetPos - _cockpit.GetPosition(), refLookAtMatrix));
YawValue = PIDControllerYaw.CalculateControlOutput(PositionToMe.X);
if (Vertical == Vector3D.Zero)
{
PitchValue = PIDControllerPitch.CalculateControlOutput(PositionToMe.Y);
}
}
if (Vertical != Vector3D.Zero)
{
MatrixD refLookAtMatrix = MatrixD.CreateLookAt(new Vector3D(), _cockpit.WorldMatrix.Down, _cockpit.WorldMatrix.Backward);
Vector3D VerticalToMe = Vector3D.Normalize(Vector3D.TransformNormal(_cockpit.GetPosition() - Vertical, refLookAtMatrix));
RollValue = PIDControllerRoll.CalculateControlOutput(VerticalToMe.X);
PitchValue = PIDControllerPitch.CalculateControlOutput(VerticalToMe.Y);
}
ApplyGyroOverride(-PitchValue, YawValue, -RollValue, Gyroscopes, _cockpit);
if (Math.Abs(YawValue) + Math.Abs(PitchValue) + Math.Abs(RollValue) < 0.05)
{
return true;
}
return false;
}
public void SetGyroOverride(bool bOverride)
{
foreach (IMyGyro g in Gyroscopes)
{
g.GyroOverride = bOverride;
}
}
void ApplyGyroOverride(double pitch_speed, double yaw_speed, double roll_speed, List<IMyGyro> gyro_list, IMyTerminalBlock reference)
{
var rotationVec = new Vector3D(pitch_speed, yaw_speed, roll_speed);
var shipMatrix = reference.WorldMatrix;
var relativeRotationVec = Vector3D.TransformNormal(rotationVec, shipMatrix);
foreach (IMyGyro thisGyro in gyro_list)
{
var gyroMatrix = thisGyro.WorldMatrix;
var transformedRotationVec = Vector3D.TransformNormal(relativeRotationVec, Matrix.Transpose(gyroMatrix));
thisGyro.Pitch = (float)transformedRotationVec.X; //because keen does some weird stuff with signs
thisGyro.Yaw = (float)transformedRotationVec.Y;
thisGyro.Roll = (float)transformedRotationVec.Z;
thisGyro.GyroOverride = true;
}
}
public class PIDGyro
{
private double kp = 1.5, kd = 1; private double prevError;
public double CalculateControlOutput(double targetPosition)
{
double error = targetPosition; double derivative = error - prevError; double output = kp * error + kd * derivative; prevError = error; return output;
}
}
}
public class MYThrusts
{
private IMyShipController _cockpit;
private MyGridProgram _program;
private double[] ThrustsPowers = new double[6];
private double[] GravityPowers = new double[6];
private List<IMyThrust> Thrusts = new List<IMyThrust>();
public PIDDistance PIDControllerX = new PIDDistance();
public PIDDistance PIDControllerY = new PIDDistance();
public PIDDistance PIDControllerZ = new PIDDistance();
public enum Direction : byte
{
Forward = 0,
Backward = 1,
Left = 2,
Right = 3,
Up = 4,
Down = 5
}
public MYThrusts(MyGridProgram program, IMyShipController cockpit)
{
_program = program;
_cockpit = cockpit;
}
public bool InitMYThrusts()
{
_program.GridTerminalSystem.GetBlocksOfType<IMyThrust>(Thrusts, Thrust => Thrust.CubeGrid.EntityId == _program.Me.CubeGrid.EntityId);
foreach (var thrust in Thrusts)
{
Base6Directions.Direction direction = _cockpit.WorldMatrix.GetClosestDirection(thrust.WorldMatrix.Backward);
thrust.CustomData = direction + "";
thrust.ThrustOverridePercentage = 0f;
thrust.ShowInTerminal = false;
thrust.Enabled = true;
byte k = (byte)(direction);
ThrustsPowers[k] += thrust.MaxEffectiveThrust;
}
PIDControllerX.Rest();
PIDControllerY.Rest();
PIDControllerZ.Rest();
for (int i = 0; i < ThrustsPowers.Length; i++)
{
if (i == 1) continue;
if (ThrustsPowers[i] == 0) return false;
}
return true;
}
//处理推进器与重力加速度
public double[] GetThrustsPowers(bool isVector = false)
{
Array.Clear(ThrustsPowers, 0, ThrustsPowers.Length);
foreach (var thrust in Thrusts)
{
Base6Directions.Direction direction = _cockpit.WorldMatrix.GetClosestDirection(thrust.WorldMatrix.Backward);
if (thrust.IsFunctional)
{
byte k = (byte)(direction);
ThrustsPowers[k] += thrust.MaxEffectiveThrust;
thrust.Enabled = true;
}
}
double ShipMass = _cockpit.CalculateShipMass().PhysicalMass;
if (ShipMass == 0) return ThrustsPowers;
for (int i = 0; i < ThrustsPowers.Length; i++)
{
ThrustsPowers[i] /= ShipMass;
}
Vector3D NaturalGravity = _cockpit.GetNaturalGravity();
if (isVector || NaturalGravity.Length() <= 0)
{
return ThrustsPowers;
}
double A_X = AngleBetweenVector(NaturalGravity, _cockpit.WorldMatrix.Left);
double A_Y = AngleBetweenVector(NaturalGravity, _cockpit.WorldMatrix.Up);
double A_Z = AngleBetweenVector(NaturalGravity, _cockpit.WorldMatrix.Forward);
//左右重力加速 左+ 右-
double F_X = NaturalGravity.Length() * Math.Cos(A_X);
//上下重力加速 上+ 下-
double F_Y = NaturalGravity.Length() * Math.Cos(A_Y);
//前后重力加速 前+ 后-
double F_Z = NaturalGravity.Length() * Math.Cos(A_Z);
GravityPowers[0] = +F_Z;
GravityPowers[1] = -F_Z;
GravityPowers[2] = +F_X;
GravityPowers[3] = -F_X;
GravityPowers[4] = +F_Y;
GravityPowers[5] = -F_Y;
for (int i = 0; i < ThrustsPowers.Length; i++)
{
ThrustsPowers[i] += GravityPowers[i];
}
return ThrustsPowers;
}
internal void SingleDirectionThrustControl(double targetPosition, double velocity, double acceleration, double deceleration, MYThrusts.Direction d1, MYThrusts.Direction d2, double maxSpeed)
{
double power = 0;
if (targetPosition < 0)
{
if (velocity <= 0)//位移与速度同向
{
double stopDistance = velocity * velocity / (2 * deceleration);//vt*vt-v0*v0=2as
targetPosition = targetPosition + stopDistance;
}
else
{
double stopDistance = velocity * velocity / (2 * acceleration);//vt*vt-v0*v0=2as
targetPosition = targetPosition - stopDistance;
}
}
else if (targetPosition > 0)
{
if (velocity > 0) //位移与速度同向
{
double stopDistance = velocity * velocity / (2 * acceleration);//vt*vt-v0*v0=2as
targetPosition = targetPosition - stopDistance;
}
else
{
double stopDistance = velocity * velocity / (2 * deceleration);//vt*vt-v0*v0=2as
targetPosition = targetPosition + stopDistance;
}
}
if (d1 == MYThrusts.Direction.Backward)
{
power = PIDControllerZ.CalculateControlOutput(targetPosition, velocity, acceleration, deceleration, maxSpeed);
}
else
if (d1 == MYThrusts.Direction.Right)
{
power = PIDControllerY.CalculateControlOutput(targetPosition, velocity, acceleration, deceleration, maxSpeed);
}
else
if (d1 == MYThrusts.Direction.Down)
{
power = PIDControllerX.CalculateControlOutput(targetPosition, velocity, acceleration, deceleration, maxSpeed);
}
if (power > 0)
{
power = power > deceleration ? deceleration : power;
}
else
{
power = power < -acceleration ? -acceleration : power;
}
SetThrustOverride(d1, d2, power);
}
internal void SetThrustOverride(Direction direction1, Direction direction2, double power)
{
byte index1 = (byte)direction1;
byte index2 = (byte)direction2;
double value1 = 0;
double value2 = 0;
if (power < 0)
{
value1 = (-power - GravityPowers[index1]) / ThrustsPowers[index1];
}
else
{
value2 = (power - GravityPowers[index2]) / ThrustsPowers[index2];
}
foreach (var thrust in Thrusts)
{
if (thrust.CustomData.Equals(direction1.ToString()))
{
if (value1 == 0)
{
thrust.ThrustOverride = (float)1 / (thrust.MaxEffectiveThrust * 10);
}
else if (value1 > 0)
{
if (thrust.ThrustOverride != (thrust.MaxThrust * value1))
{ thrust.ThrustOverride = (float)(thrust.MaxEffectiveThrust * value1); }
}
}
if (thrust.CustomData.Equals(direction2.ToString()))
{
if (value2 == 0)
{
thrust.ThrustOverride = (float)1 / (thrust.MaxEffectiveThrust * 10);
}
else if (value2 > 0)
{
if (thrust.ThrustOverride != (thrust.MaxThrust * value2))
{ thrust.ThrustOverride = (float)(thrust.MaxEffectiveThrust * value2); }
}
}
}
}
internal void SetThrustOverrideByDirection(Direction direction, double Value)
{
foreach (var thrust in Thrusts)
{
if (!thrust.CustomData.Equals(direction.ToString())) continue;
if (thrust.ThrustOverride != (thrust.MaxThrust * Value))
{ thrust.ThrustOverride = (float)(thrust.MaxEffectiveThrust * Value); }
}
}
internal void resetThrustOverride(bool Enabled = true)
{
foreach (var thrust in Thrusts)
{
thrust.ThrustOverridePercentage = 0f;
thrust.Enabled = Enabled;
}
}
internal double AngleBetweenVector(Vector3D V_A, Vector3D V_B)
{
return Math.Acos(Vector3D.Dot(V_A, V_B) / (V_A.Length() * V_B.Length()));
}
public class PIDDistance
{
private double kp = 4, kd = 3;
private double prevError;
private List<double> error = new List<double>();
private PIDSpeed mPIDSpeed = new PIDSpeed();
public double CalculateControlOutput(double targetPosition, double velocity, double acceleration, double deceleration, double maxSpeed)
{
double error = targetPosition;
double derivative = error - prevError;
double targetSpeed = kp * error + kd * derivative;
prevError = error;
if (targetSpeed > 0)
{
targetSpeed = targetSpeed > maxSpeed ? maxSpeed : targetSpeed;
}
if (targetSpeed < 0)
{
targetSpeed = targetSpeed < -maxSpeed ? -maxSpeed : targetSpeed;
}
double output = mPIDSpeed.CalculateOutput(velocity, targetSpeed, maxSpeed);
return output;
}
public void Rest()
{
prevError = 0;
mPIDSpeed.Rest();
}
}
public class PIDSpeed
{
private double kp = 2.5, ki = 0.05, kd = 1.5;
private double prevError, integral;
private int KT = 0;
private double derivative;
public double CalculateOutput(double currentSpeed, double targetSpeed, double maxSpeed)
{
double error = targetSpeed - currentSpeed;
KT += 1;
integral = (integral + error) / KT;
integral += error;
derivative = error - prevError;
double output = kp * error + ki * integral + kd * derivative;
prevError = error;
return output;
}
public void Rest()
{
prevError = 0;
integral = 0;
KT = 0;
}
}
}
#endregion
#endregion
太空工程师导弹脚本-ADS|Air defense System
于 2025-03-10 13:02:08 首次发布
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
