· 6 years ago · Jan 10, 2020, 03:00 AM
1//------------------------------------------------------------
2// ADN - Trajectory Missile Script v3.4
3//------------------------------------------------------------
4
5//Type of block to disconnect missile from launching ship: 0 = Merge Block, 1 = Rotor, 2 = Connector, 3 = Merge Block And Any Locked Connectors, 4 = Rotor And Any Locked Connectors, 99 = No detach required
6int missileDetachPortType = 0;
7
8//Type of missile trajectory: 0 = Frefall To Target With Aiming (For Cluster Bomb Deployments), 1 = Frefall To Target Without Aiming (For Cluster Bomb Deployments), 2 = Thrust And Home In To Target
9int missileTrajectoryType = 0;
10
11//Script will only extract missile blocks on same grid as this PB
12bool missileBlockSameGridOnly = true;
13
14//------------------------------ Reference Block Name Configuration ------------------------------
15
16string strShipRefTargetPanel = "R_TARGET"; //For changing target location or issue command in-flight
17
18//By default all gyroscopes, thrusters and merge blocks will be considered for use. Setting a value here limits the script to use specific set of blocks
19string strGyroscopesTag = "";
20string strThrustersTag = "";
21string strDetachPortTag = "";
22string strDirectionRefBlockTag = "";
23
24//For debugging purposes
25string strStatusDisplayPrefix = "<D>";
26
27//------------------------------ Missile Handling Configuration ------------------------------
28
29double driftVectorReduction = 1.5;
30double launchSeconds = 5;
31double missileTravelHeight = 4000; //The height in metres the missile will try to reach, measuring from launch point
32
33int missileDeployDistanceCorrection = 0; //Deploy missile this distance earlier in metres
34string missileDeployCommand = "";
35
36bool? boolDrift = null;
37bool? boolLeadTarget = null;
38bool? boolNaturalDampener = null;
39
40//------------------------------ This Section Is Missile Turning Parameters ------------------------------
41
42double MAX_FALL_SPEED = 104.37; //Maximum falling speed. If speed mod is used, this is the maximum speed of the speed mod
43int HEIGHT_DEAD_ZONE = 500; //Height range above the cruise height where missile will not perform any tangent vector adjustment to prevent oscillation
44
45bool readjustMaxFallSpeed = true; //If speed mods are used, user may forget to configure MAX_FALL_SPEED. This flag will attempt to readjust it based on current missile speed
46
47//------------------------------ Above Is User Configuration Section. This Section Is For PID Tuning ------------------------------
48
49double AIM_P = 0;
50double AIM_I = 0;
51double AIM_D = 0;
52double AIM_LIMIT = 6.3;
53
54double INTEGRAL_WINDUP_UPPER_LIMIT = 0;
55double INTEGRAL_WINDUP_LOWER_LIMIT = 0;
56
57//------------------------------ Script Parameters Configuration ------------------------------
58
59int MERGE_SEPARATE_WAIT_THRESHOLD = 60;
60
61bool outputMissileStatus = false;
62
63//------------------------------ Important Constants ------------------------------
64
65const double DEF_SMALL_GRID_P = 31.42;
66const double DEF_SMALL_GRID_I = 0;
67const double DEF_SMALL_GRID_D = 10.48;
68
69const double DEF_BIG_GRID_P = 15.71;
70const double DEF_BIG_GRID_I = 0;
71const double DEF_BIG_GRID_D = 7.05;
72
73const float SECOND = 60f;
74
75//------------------------------ Below Is Main Script Body ------------------------------
76
77IMyTextPanel shipRefTargetPanel;
78
79IMyShipController remoteControl;
80IMyTerminalBlock refForwardBlock;
81IMyTerminalBlock refDownwardBlock;
82IMyTerminalBlock statusDisplay;
83
84GyroControl gyroControl;
85
86List<IMyTerminalBlock> thrusters;
87float[] thrustValues;
88
89MatrixD refWorldMatrix;
90MatrixD refViewMatrix;
91bool refForwardReverse;
92
93Vector3D naturalGravity;
94double naturalGravityLength;
95
96Vector3D referencePoint; //Coordinates of the planet center
97double cruiseHeight; //The height from planet center the missile should cruise at
98double cruiseHeightPeak; //The cut off height from planet center the missile will start to reduce height
99
100Vector3D driftVector;
101double speed;
102double rpm;
103
104double lastSpeed;
105Vector3D lastPosition;
106
107Vector3D targetPosition;
108Vector3D lastTargetPosition;
109
110bool targetPositionSet;
111
112Vector3D targetVector;
113double distToTarget;
114
115Vector3D impactPoint;
116double timeToImpact;
117
118PIDController yawController;
119PIDController pitchController;
120PIDController rollController;
121
122bool spinActive = false;
123
124int subCounter = 0;
125int subMode = 0;
126int mode = 0;
127int clock = 0;
128bool init = false;
129
130IMyTerminalBlock detachBlock;
131int detachBlockType = -1;
132
133List<KeyValuePair<double, string[]>> rpmTriggerList;
134List<KeyValuePair<double, string[]>> distTriggerList;
135List<KeyValuePair<int, string[]>> timeTriggerList;
136Dictionary<string, List<IMyTerminalBlock>> savedBlockList;
137
138Random rnd = new Random();
139
140void Main(string arguments, UpdateType updateSource)
141{
142 //---------- Initialization And General Controls ----------
143
144 if (!init)
145 {
146 if (subMode == 0) //Check for configuration command
147 {
148 subMode = 1;
149
150 if (Me.CustomData.Length > 0)
151 {
152 ProcessCustomConfiguration();
153 }
154
155 if (arguments.Length > 0)
156 {
157 ProcessConfigurationCommand(arguments);
158
159 if (!targetPositionSet)
160 {
161 return;
162 }
163 }
164 }
165
166 if (subMode == 1) //Missile still on launching ship's grid
167 {
168 InitLaunchingShipRefBlocks();
169
170 if (shipRefTargetPanel == null)
171 {
172 boolLeadTarget = false;
173 }
174
175 if (!targetPositionSet && !PopulateTargetLocation(arguments))
176 {
177 if (shipRefTargetPanel != null)
178 {
179 if (!PopulateTargetLocation(shipRefTargetPanel.GetPublicTitle()))
180 {
181 return;
182 }
183 }
184 else
185 {
186 return;
187 }
188 }
189
190 Runtime.UpdateFrequency = UpdateFrequency.Update1;
191
192 if (!DetachFromGrid())
193 {
194 throw new Exception("--- Initialization Failed ---");
195 }
196
197 subCounter = 0;
198 subMode = (missileDetachPortType == 99 ? 3 : 2);
199 return;
200 }
201 else if (subMode == 2) //Missile waiting for successful detachment from launching ship
202 {
203 bool isDetached = false;
204
205 if (detachBlockType == 0)
206 {
207 isDetached = !((detachBlock as IMyShipMergeBlock).IsConnected);
208 }
209 else if (detachBlockType == 1)
210 {
211 isDetached = !((detachBlock as IMyMotorBase).IsAttached);
212 }
213 else if (detachBlockType == 2)
214 {
215 isDetached = ((detachBlock as IMyShipConnector).Status != MyShipConnectorStatus.Connected);
216 }
217
218 if (isDetached)
219 {
220 subMode = 3;
221 return;
222 }
223 else
224 {
225 subCounter++;
226
227 if (subCounter >= MERGE_SEPARATE_WAIT_THRESHOLD)
228 {
229 Echo("Error: Missile detach failed.");
230 throw new Exception("--- Initialization Failed ---");
231 }
232
233 return;
234 }
235 }
236 else if (subMode == 3) //Missile successfully detached and currently initializing
237 {
238 if (missileDetachPortType == 3 || missileDetachPortType == 4)
239 {
240 DetachLockedConnectors();
241 }
242
243 if (!InitMissileBlocks())
244 {
245 throw new Exception("--- Initialization Failed ---");
246 }
247 }
248
249 gyroControl.Enabled(true);
250
251 lastPosition = refWorldMatrix.Translation;
252
253 subCounter = (int)(launchSeconds * SECOND);
254 FireThrusters();
255
256 subMode = 0;
257 mode = 1;
258 clock = 0;
259
260 init = true;
261 return;
262 }
263
264 //---------- Modes And Controls ----------
265
266 bool allowDisplayUpdate = true;
267
268 if (shipRefTargetPanel != null)
269 {
270 targetPositionSet = PopulateTargetLocation(shipRefTargetPanel.GetPublicTitle());
271 ExecuteTargetCommand(shipRefTargetPanel.CustomData);
272 }
273
274 if ((updateSource & UpdateType.Update1) == 0 || Runtime.TimeSinceLastRun.Ticks == 0)
275 {
276 return;
277 }
278
279 clock++;
280
281 CalculateParameters();
282
283 if (naturalGravityLength < 0.01)
284 {
285 return;
286 }
287
288 if (mode == 1)
289 {
290 FireThrusters();
291
292 if (subCounter > 0)
293 {
294 subCounter -= 1;
295 }
296 else
297 {
298 gyroControl.SetGyroOverride(true);
299
300 subCounter = 0;
301 mode = 2;
302 }
303 }
304 else if (mode == 2)
305 {
306 if (clock % SECOND == 0)
307 {
308 FireThrusters();
309 }
310
311 targetVector = -naturalGravity;
312 distToTarget = naturalGravityLength;
313
314 targetVector = Vector3D.TransformNormal(targetVector, refViewMatrix);
315 targetVector.Normalize();
316
317 AimAtTarget();
318
319 if (boolNaturalDampener == true)
320 {
321 Vector3D alignTarget = Vector3D.Normalize(targetPosition - refWorldMatrix.Translation);
322 AimDampenerAtVector(ref alignTarget);
323 }
324
325 if ((refWorldMatrix.Translation - lastPosition).Length() > missileTravelHeight - (speed * speed / naturalGravityLength / 2))
326 {
327 remoteControl.TryGetPlanetPosition(out referencePoint);
328
329 cruiseHeight = (lastPosition - referencePoint).Length() + missileTravelHeight;
330 cruiseHeightPeak = cruiseHeight + HEIGHT_DEAD_ZONE;
331
332 subCounter = 0;
333 mode = 4;
334 }
335 }
336 else if (mode == 3)
337 {
338 if (clock % SECOND == 0)
339 {
340 FireThrusters();
341 }
342
343 Vector3D vectorN = targetPosition - refWorldMatrix.Translation;
344 vectorN.Normalize();
345
346 targetVector = Vector3D.Reject(vectorN, naturalGravity);
347 targetVector.Normalize();
348
349 distToTarget = targetVector.Length();
350 targetVector = targetVector / distToTarget;
351
352 targetVector = Vector3D.TransformNormal(targetVector, refViewMatrix);
353 targetVector.Normalize();
354
355 AimAtTarget();
356
357 if (boolNaturalDampener == true)
358 {
359 AimDampenerAtVector(ref naturalGravity);
360 }
361
362 subCounter = 0;
363 mode = 4;
364 }
365 else if (mode == 4)
366 {
367 if (clock % SECOND == 0)
368 {
369 FireThrusters();
370 }
371
372 Vector3D vectorN = targetPosition - refWorldMatrix.Translation;
373 vectorN.Normalize();
374
375 targetVector = Vector3D.Reject(vectorN, naturalGravity);
376 targetVector.Normalize();
377
378 double currentHeight = (referencePoint - refWorldMatrix.Translation).Length();
379 if (currentHeight > cruiseHeightPeak)
380 {
381 targetVector += (naturalGravity / Math.Tan(Math.Asin(cruiseHeightPeak / currentHeight)));
382 }
383 else if (currentHeight < cruiseHeight)
384 {
385 targetVector -= (naturalGravity / Math.Tan(Math.Asin(currentHeight / cruiseHeight)));
386 }
387
388 distToTarget = targetVector.Length();
389 targetVector = targetVector / distToTarget;
390
391 if (boolDrift == true && speed >= 5)
392 {
393 if (driftVector.Dot(naturalGravity) < 0)
394 {
395 targetVector = (targetVector * speed) - (Vector3D.Reject(driftVector, naturalGravity) / driftVectorReduction);
396 }
397 else
398 {
399 targetVector = (targetVector * speed) - (driftVector / driftVectorReduction);
400 }
401 targetVector.Normalize();
402 }
403
404 targetVector = Vector3D.TransformNormal(targetVector, refViewMatrix);
405 targetVector.Normalize();
406
407 AimAtTarget();
408
409 if (boolNaturalDampener == true)
410 {
411 AimDampenerAtVector(ref naturalGravity);
412 }
413
414 if (readjustMaxFallSpeed && speed > MAX_FALL_SPEED + 0.1)
415 {
416 MAX_FALL_SPEED = speed;
417 }
418
419 Vector3D targetHeightVector = (targetPosition - referencePoint);
420 double targetHeightLength = targetHeightVector.Length();
421 targetHeightVector = targetHeightVector / targetHeightLength;
422 Vector3D missileSurfacePoint = referencePoint - (naturalGravity * targetHeightLength);
423
424 double landDistance = (missileSurfacePoint - targetPosition).Length();
425 double dropDistance = currentHeight - targetHeightLength;
426 double relativeImpactSpeed = Math.Sqrt(2 * dropDistance * naturalGravityLength);
427 double dropTime = relativeImpactSpeed / naturalGravityLength;
428 double largestPossibleDistance = speed * dropTime * 2;
429
430 if (landDistance - missileDeployDistanceCorrection <= largestPossibleDistance)
431 {
432 SimulateImpactPoint(referencePoint, naturalGravityLength, targetPosition, refWorldMatrix.Translation, driftVector, MAX_FALL_SPEED, 0.01, 3000, out impactPoint, out timeToImpact);
433 if ((impactPoint - missileSurfacePoint).Length() >= landDistance - missileDeployDistanceCorrection)
434 {
435 subCounter = 0;
436 mode = 5;
437 }
438 }
439 }
440 else if (mode == 5)
441 {
442 if (missileTrajectoryType <= 1)
443 {
444 DisableAllThrusters();
445 }
446
447 ExecuteTargetCommand(missileDeployCommand);
448
449 lastTargetPosition = targetPosition;
450
451 if (missileTrajectoryType == 1)
452 {
453 gyroControl.ZeroTurnGyro();
454 }
455
456 subCounter = 0;
457 mode = 6;
458 }
459 else if (mode == 6)
460 {
461 targetVector = targetPosition - refWorldMatrix.Translation;
462 distToTarget = targetVector.Length();
463 targetVector = targetVector / distToTarget;
464
465 if (missileTrajectoryType == 2)
466 {
467 if (boolDrift == true && speed >= 5)
468 {
469 targetVector = (targetVector * speed) - (driftVector / driftVectorReduction);
470 targetVector.Normalize();
471 }
472 }
473
474 targetVector = Vector3D.TransformNormal(targetVector, refViewMatrix);
475 targetVector.Normalize();
476
477 if (missileTrajectoryType != 1)
478 {
479 AimAtTarget();
480 }
481
482 if (missileTrajectoryType == 2 && !spinActive)
483 {
484 if (boolNaturalDampener == true)
485 {
486 AimDampenerAtVector(ref naturalGravity);
487 }
488 }
489
490 distToTarget = (targetPosition - refWorldMatrix.Translation).Length();
491
492 if (rpmTriggerList != null && rpmTriggerList.Count > 0)
493 {
494 int i = 0;
495 while (i < rpmTriggerList.Count)
496 {
497 if (rpmTriggerList[i].Key <= rpm)
498 {
499 ProcessSingleCommand(rpmTriggerList[i].Value);
500 rpmTriggerList.RemoveAt(i);
501 }
502 else
503 {
504 i++;
505 }
506 }
507 }
508
509 if (distTriggerList != null && distTriggerList.Count > 0)
510 {
511 int i = 0;
512 while (i < distTriggerList.Count)
513 {
514 if (distTriggerList[i].Key >= distToTarget)
515 {
516 ProcessSingleCommand(distTriggerList[i].Value);
517 distTriggerList.RemoveAt(i);
518 }
519 else
520 {
521 i++;
522 }
523 }
524 }
525
526 if (timeTriggerList != null && timeTriggerList.Count > 0)
527 {
528 int i = 0;
529 while (i < timeTriggerList.Count)
530 {
531 if (timeTriggerList[i].Key <= clock)
532 {
533 ProcessSingleCommand(timeTriggerList[i].Value);
534 timeTriggerList.RemoveAt(i);
535 }
536 else
537 {
538 i++;
539 }
540 }
541 }
542 }
543
544 if (statusDisplay != null && allowDisplayUpdate)
545 {
546 if (mode == 0)
547 {
548 DisplayStatus("Idle");
549 }
550 else if (mode == 1)
551 {
552 DisplayStatus("Launching");
553 }
554 else if (mode == 2)
555 {
556 DisplayStatus("Gaining Altitude");
557 }
558 else if (mode == 3)
559 {
560 DisplayStatus("Flight Calibration");
561 }
562 else if (mode == 4)
563 {
564 DisplayStatus("Enroute To Target");
565 }
566 else if (mode == 5)
567 {
568 DisplayStatus("Decceleration");
569 }
570 else if (mode == 6)
571 {
572 DisplayStatus("Approach Alignment");
573 }
574 else if (mode == 7 || mode == 8 || mode == 9)
575 {
576 DisplayStatus("Deploy");
577 }
578 else if (mode == 17 || mode == 18)
579 {
580 DisplayStatus("Homing Descend");
581 }
582 else
583 {
584 DisplayStatus("-");
585 }
586 }
587
588 if (outputMissileStatus)
589 {
590 Echo("ST:" + mode + ":" + subMode + ":" + subCounter + ":" + clock + ":-:" +
591 Math.Round(targetPosition.GetDim(0), 5) + ":" + Math.Round(targetPosition.GetDim(1), 5) + ":" + Math.Round(targetPosition.GetDim(2), 5) + ":" +
592 0 + ":");
593 }
594}
595
596//------------------------------ Miscellaneous Methods ------------------------------
597
598void DisplayStatus(string statusMsg)
599{
600 if (statusDisplay != null)
601 {
602 statusDisplay.CustomName = strStatusDisplayPrefix + " Mode: " + mode + ", " + statusMsg;
603 }
604}
605
606//------------------------------ Missile And Target Information Methods ------------------------------
607
608void SimulateImpactPoint(Vector3D planetCenter, double planetGravity, Vector3D targetPoint, Vector3D currentPoint, Vector3D currentDirection, double maxSpeedLimit, double angleLimit, int loopLimit, out Vector3D vImpactPoint, out double nTimeToImpact)
609{
610 double targetHeight = (planetCenter - targetPoint).Length();
611 double acosAngleLimit = Math.Cos(angleLimit);
612 bool hitMaxSpeed = false;
613
614 for (int i = 0; i < loopLimit; i++)
615 {
616 Vector3D centerToCurrent = currentPoint - planetCenter;
617 double currentImpactHeight = centerToCurrent.Length();
618 if (currentImpactHeight <= targetHeight)
619 {
620 vImpactPoint = currentPoint;
621 nTimeToImpact = (i / 60.0);
622
623 return;
624 }
625
626 Vector3D gravityVector = Vector3D.Normalize(planetCenter - currentPoint) * planetGravity;
627
628 if (hitMaxSpeed && angleLimit > 0)
629 {
630 double acosCurrentAngle = currentDirection.Dot(gravityVector) / planetGravity / currentDirection.Length();
631 if (acosCurrentAngle >= acosAngleLimit)
632 {
633 vImpactPoint = (centerToCurrent / currentImpactHeight * targetHeight) + planetCenter;
634 nTimeToImpact = (i / 60.0) + (Math.Abs(currentImpactHeight - targetHeight) / maxSpeedLimit);
635
636 return;
637 }
638 }
639
640 currentDirection += (gravityVector / 60);
641 if (maxSpeedLimit > 0)
642 {
643 double speed = currentDirection.Length();
644 if (speed > maxSpeedLimit)
645 {
646 currentDirection = currentDirection / speed * maxSpeedLimit;
647 hitMaxSpeed = true;
648 }
649
650 currentPoint += (currentDirection / 60);
651 }
652 }
653
654 Vector3D centerToFinalPoint = currentPoint - planetCenter;
655 double finalImpactHeight = centerToFinalPoint.Length();
656
657 vImpactPoint = (centerToFinalPoint / finalImpactHeight * targetHeight) + planetCenter;
658 nTimeToImpact = (loopLimit / 60.0) + (Math.Abs(finalImpactHeight - targetHeight) / maxSpeedLimit);
659}
660
661void CalculateParameters()
662{
663 //---------- Calculate Missile Related Variables ----------
664
665 refWorldMatrix = refForwardBlock.WorldMatrix;
666 if (refForwardReverse)
667 {
668 refWorldMatrix.Forward = refWorldMatrix.Backward;
669 refWorldMatrix.Left = refWorldMatrix.Right;
670 }
671 refViewMatrix = MatrixD.Transpose(refWorldMatrix);
672
673 lastSpeed = speed;
674 driftVector = remoteControl.GetShipVelocities().LinearVelocity;
675 speed = driftVector.Length();
676
677 rpm = Math.Abs(remoteControl.GetShipVelocities().AngularVelocity.Dot(refWorldMatrix.Forward)) * MathHelper.RadiansPerSecondToRPM;
678
679 naturalGravity = remoteControl.GetNaturalGravity();
680 naturalGravityLength = naturalGravity.Length();
681 naturalGravity = (naturalGravityLength > 0 ? naturalGravity / naturalGravityLength : Vector3D.Zero);
682}
683
684//------------------------------ Missile Aiming Methods ------------------------------
685
686int GetMultiplierSign(double value)
687{
688 return (value < 0 ? -1 : 1);
689}
690
691void AimAtTarget()
692{
693 //---------- Activate Gyroscopes To Turn Towards Target ----------
694
695 Vector3D yawVector = new Vector3D(targetVector.GetDim(0), 0, targetVector.GetDim(2));
696 Vector3D pitchVector = new Vector3D(0, targetVector.GetDim(1), targetVector.GetDim(2));
697 yawVector.Normalize();
698 pitchVector.Normalize();
699
700 double yawInput = Math.Acos(yawVector.Dot(Vector3D.Forward)) * GetMultiplierSign(targetVector.GetDim(0));
701 double pitchInput = Math.Acos(pitchVector.Dot(Vector3D.Forward)) * GetMultiplierSign(targetVector.GetDim(1));
702
703 //---------- PID Controller Adjustment ----------
704
705 yawInput = yawController.Filter(yawInput, 2);
706 pitchInput = pitchController.Filter(pitchInput, 2);
707
708 if (Math.Abs(yawInput) + Math.Abs(pitchInput) > AIM_LIMIT)
709 {
710 double adjust = AIM_LIMIT / (Math.Abs(yawInput) + Math.Abs(pitchInput));
711 yawInput *= adjust;
712 pitchInput *= adjust;
713 }
714
715 //---------- Set Gyroscope Parameters ----------
716
717 gyroControl.SetGyroYaw((float)yawInput);
718 gyroControl.SetGyroPitch((float)pitchInput);
719}
720
721void AimDampenerAtVector(ref Vector3D aimVector)
722{
723 //---------- Activate Gyroscopes To Aim Dampener At Natural Gravity ----------
724
725 if (refDownwardBlock == null || naturalGravityLength < 0.01)
726 {
727 return;
728 }
729
730 MatrixD dampenerLookAtMatrix = MatrixD.CreateLookAt(Vector3D.Zero, refDownwardBlock.WorldMatrix.Forward, refWorldMatrix.Forward);
731
732 Vector3D gravityVector = Vector3D.TransformNormal(aimVector, dampenerLookAtMatrix);
733 gravityVector.SetDim(1, 0);
734 gravityVector.Normalize();
735
736 if (Double.IsNaN(gravityVector.Sum))
737 {
738 gravityVector = Vector3D.Forward;
739 }
740
741 double rollInput = Math.Acos(gravityVector.Dot(Vector3D.Forward)) * GetMultiplierSign(gravityVector.GetDim(0));
742
743 //---------- PID Controller Adjustment ----------
744
745 rollInput = rollController.Filter(rollInput, 2);
746
747 //---------- Set Gyroscope Parameters ----------
748
749 gyroControl.SetGyroRoll((float)rollInput);
750}
751
752//------------------------------ Missile Separation Methods ------------------------------
753
754bool DetachFromGrid(bool testOnly = false)
755{
756 List<IMyTerminalBlock> blocks;
757
758 switch (missileDetachPortType)
759 {
760 case 0:
761 case 3:
762 blocks = (strDetachPortTag != null && strDetachPortTag.Length > 0 ? GetBlocksWithName<IMyShipMergeBlock>(strDetachPortTag) : GetBlocksOfType<IMyShipMergeBlock>());
763 detachBlock = GetClosestBlockFromReference(blocks, Me, true);
764
765 if (!testOnly)
766 {
767 if (detachBlock == null)
768 {
769 Echo("Error: Missing Merge Block " + (strDetachPortTag != null && strDetachPortTag.Length > 0 ? "with tag " + strDetachPortTag + " to detach" : "to detach."));
770 return false;
771 }
772 detachBlockType = 0;
773
774 detachBlock.ApplyAction("OnOff_Off");
775 }
776 return true;
777 case 1:
778 case 4:
779 blocks = (strDetachPortTag != null && strDetachPortTag.Length > 0 ? GetBlocksWithName<IMyMotorBase>(strDetachPortTag) : GetBlocksOfType<IMyMotorBase>());
780 for (int i = 0; i < blocks.Count; i++)
781 {
782 IMyCubeGrid grid = ((IMyMotorBase)blocks[i]).TopGrid;
783 if (grid != null && grid == Me.CubeGrid)
784 {
785 detachBlock = blocks[i];
786 break;
787 }
788 }
789
790 if (detachBlock == null)
791 {
792 for (int i = 0; i < blocks.Count; i++)
793 {
794 if (blocks[i].CubeGrid == Me.CubeGrid)
795 {
796 detachBlock = blocks[i];
797 break;
798 }
799 }
800 }
801
802 if (!testOnly)
803 {
804 if (detachBlock == null)
805 {
806 Echo("Error: Missing Rotor " + (strDetachPortTag != null && strDetachPortTag.Length > 0 ? "with tag " + strDetachPortTag + " to detach" : "to detach."));
807 return false;
808 }
809 detachBlockType = 1;
810
811 detachBlock.ApplyAction("Detach");
812 }
813 return true;
814 case 2:
815 blocks = (strDetachPortTag != null && strDetachPortTag.Length > 0 ? GetBlocksWithName<IMyShipConnector>(strDetachPortTag) : GetBlocksOfType<IMyShipConnector>());
816 detachBlock = GetClosestBlockFromReference(blocks, Me, true);
817
818 if (!testOnly)
819 {
820 if (detachBlock == null)
821 {
822 Echo("Error: Missing Connector " + (strDetachPortTag != null && strDetachPortTag.Length > 0 ? "with tag " + strDetachPortTag + " to detach" : "to detach."));
823 return false;
824 }
825 detachBlockType = 2;
826
827 detachBlock.ApplyAction("Unlock");
828 }
829 return true;
830 case 99:
831 return true;
832 default:
833 if (!testOnly)
834 {
835 Echo("Error: Unknown missileDetachPortType - " + missileDetachPortType + ".");
836 }
837 return false;
838 }
839}
840
841IMyTerminalBlock GetClosestBlockFromReference(List<IMyTerminalBlock> checkBlocks, IMyTerminalBlock referenceBlock, bool sameGridCheck = false)
842{
843 IMyTerminalBlock checkBlock = null;
844 double prevCheckDistance = Double.MaxValue;
845
846 for (int i = 0; i < checkBlocks.Count; i++)
847 {
848 if (!sameGridCheck || checkBlocks[i].CubeGrid == referenceBlock.CubeGrid)
849 {
850 double currCheckDistance = (checkBlocks[i].GetPosition() - referenceBlock.GetPosition()).Length();
851 if (currCheckDistance < prevCheckDistance)
852 {
853 prevCheckDistance = currCheckDistance;
854 checkBlock = checkBlocks[i];
855 }
856 }
857 }
858
859 return checkBlock;
860}
861
862IMyTerminalBlock GetConnectedMergeBlock(IMyCubeGrid grid, IMyTerminalBlock mergeBlock)
863{
864 IMySlimBlock slimBlock = grid.GetCubeBlock(mergeBlock.Position - new Vector3I(Base6Directions.GetVector(mergeBlock.Orientation.Left)));
865 return (slimBlock == null ? null : slimBlock.FatBlock as IMyTerminalBlock);
866}
867
868void DetachLockedConnectors()
869{
870 List<IMyTerminalBlock> blocks = GetBlocksOfType<IMyShipConnector>();
871 for (int i = 0; i < blocks.Count; i++)
872 {
873 if (blocks[i].CubeGrid == Me.CubeGrid)
874 {
875 IMyShipConnector otherConnector = ((IMyShipConnector)blocks[i]).OtherConnector;
876 if (otherConnector == null || blocks[i].CubeGrid != otherConnector.CubeGrid)
877 {
878 blocks[i].ApplyAction("Unlock");
879 }
880 }
881 }
882}
883
884//------------------------------ Command Processing Methods ------------------------------
885
886bool PopulateTargetLocation(string command)
887{
888 if (command == null || command.Length == 0)
889 {
890 return false;
891 }
892
893 string[] tokens = command.Trim().Split(':');
894 if (tokens[0].Trim().Equals("GPS") && tokens.Length > 4)
895 {
896 Vector3D parsedVector = new Vector3D();
897 double result;
898
899 if (Double.TryParse(tokens[2], out result))
900 {
901 parsedVector.SetDim(0, result);
902 }
903 else
904 {
905 return false;
906 }
907
908 if (Double.TryParse(tokens[3], out result))
909 {
910 parsedVector.SetDim(1, result);
911 }
912 else
913 {
914 return false;
915 }
916
917 if (Double.TryParse(tokens[4], out result))
918 {
919 parsedVector.SetDim(2, result);
920 }
921 else
922 {
923 return false;
924 }
925
926 targetPosition = parsedVector;
927 return true;
928 }
929 else
930 {
931 return false;
932 }
933}
934
935void ExecuteTargetCommand(string commandLine)
936{
937 string[] keyValues = commandLine.Split(',');
938
939 for (int i = 0; i < keyValues.Length; i++)
940 {
941 string[] tokens = keyValues[i].Trim().Split(':');
942 if (tokens.Length > 0)
943 {
944 ProcessSingleCommand(tokens);
945 }
946 }
947}
948
949void ProcessSingleCommand(string[] tokens)
950{
951 string cmdToken = tokens[0].Trim();
952 if (cmdToken.StartsWith("ACT") && tokens.Length >= 3)
953 {
954 char opCode = (cmdToken.Length >= 4 ? cmdToken[3] : 'B');
955 List<IMyTerminalBlock> triggerBlocks = null;
956 switch (opCode)
957 {
958 case 'B':
959 triggerBlocks = GetBlocksWithName<IMyTerminalBlock>(tokens[1], 3);
960 break;
961 case 'P':
962 triggerBlocks = GetBlocksWithName<IMyTerminalBlock>(tokens[1], 1);
963 break;
964 case 'S':
965 triggerBlocks = GetBlocksWithName<IMyTerminalBlock>(tokens[1], 2);
966 break;
967 case 'W':
968 triggerBlocks = GetBlocksWithName<IMyTerminalBlock>(tokens[1], 0);
969 break;
970 case 'C':
971 triggerBlocks = (savedBlockList != null ? savedBlockList[tokens[1]] : null);
972 break;
973 }
974
975 if (triggerBlocks != null)
976 {
977 for (int i = 0; i < triggerBlocks.Count; i++)
978 {
979 ITerminalAction action = triggerBlocks[i].GetActionWithName(tokens[2]);
980 if (action != null)
981 {
982 action.Apply(triggerBlocks[i]);
983 }
984 }
985 }
986 }
987 else if (cmdToken.StartsWith("SET") && tokens.Length >= 3)
988 {
989 char opCode = (cmdToken.Length >= 4 ? cmdToken[3] : 'B');
990 List<IMyTerminalBlock> triggerBlocks = null;
991 switch (opCode)
992 {
993 case 'B':
994 triggerBlocks = GetBlocksWithName<IMyTerminalBlock>(tokens[1], 3);
995 break;
996 case 'P':
997 triggerBlocks = GetBlocksWithName<IMyTerminalBlock>(tokens[1], 1);
998 break;
999 case 'S':
1000 triggerBlocks = GetBlocksWithName<IMyTerminalBlock>(tokens[1], 2);
1001 break;
1002 case 'W':
1003 triggerBlocks = GetBlocksWithName<IMyTerminalBlock>(tokens[1], 0);
1004 break;
1005 case 'C':
1006 triggerBlocks = (savedBlockList != null ? savedBlockList[tokens[1]] : null);
1007 break;
1008 }
1009
1010 char propCode = (cmdToken.Length >= 5 ? cmdToken[4] : 'P');
1011
1012 if (triggerBlocks != null)
1013 {
1014 for (int i = 0; i < triggerBlocks.Count; i++)
1015 {
1016 switch (propCode)
1017 {
1018 case 'P':
1019 triggerBlocks[i].SetValueFloat(tokens[2], float.Parse(tokens[3]));
1020 break;
1021 case 'B':
1022 triggerBlocks[i].SetValueBool(tokens[2], bool.Parse(tokens[3]));
1023 break;
1024 case 'D':
1025 triggerBlocks[i].SetValueFloat(tokens[2], (float)distToTarget / float.Parse(tokens[3]));
1026 break;
1027 case 'S':
1028 triggerBlocks[i].SetValueFloat(tokens[2], (float)speed / float.Parse(tokens[3]));
1029 break;
1030 case 'T':
1031 triggerBlocks[i].SetValueFloat(tokens[2], (float)(timeToImpact <= 0 ? (distToTarget / speed) : timeToImpact) / float.Parse(tokens[3]));
1032 break;
1033 case 'A':
1034 triggerBlocks[i].SetValueFloat(tokens[2], triggerBlocks[i].GetValueFloat(tokens[2]) + float.Parse(tokens[3]));
1035 break;
1036 case 'M':
1037 triggerBlocks[i].SetValueFloat(tokens[2], triggerBlocks[i].GetValueFloat(tokens[2]) * float.Parse(tokens[3]));
1038 break;
1039 }
1040 }
1041 }
1042 }
1043 else if (cmdToken.Equals("TGR") && tokens.Length >= 3)
1044 {
1045 if (rpmTriggerList == null)
1046 {
1047 rpmTriggerList = new List<KeyValuePair<double, string[]>>();
1048 }
1049
1050 string[] items = new string[tokens.Length - 2];
1051 Array.Copy(tokens, 2, items, 0, items.Length);
1052 rpmTriggerList.Add(new KeyValuePair<double, string[]>(Double.Parse(tokens[1]), items));
1053 }
1054 else if (cmdToken.Equals("TGD") && tokens.Length >= 3)
1055 {
1056 if (distTriggerList == null)
1057 {
1058 distTriggerList = new List<KeyValuePair<double, string[]>>();
1059 }
1060
1061 string[] items = new string[tokens.Length - 2];
1062 Array.Copy(tokens, 2, items, 0, items.Length);
1063 distTriggerList.Add(new KeyValuePair<double, string[]>(Double.Parse(tokens[1]), items));
1064 }
1065 else if (cmdToken.Equals("TGE") && tokens.Length >= 3)
1066 {
1067 if (distTriggerList == null)
1068 {
1069 distTriggerList = new List<KeyValuePair<double, string[]>>();
1070 }
1071
1072 string[] items = new string[tokens.Length - 2];
1073 Array.Copy(tokens, 2, items, 0, items.Length);
1074 distTriggerList.Add(new KeyValuePair<double, string[]>(distToTarget - Double.Parse(tokens[1]), items));
1075 }
1076 else if (cmdToken.Equals("TGT") && tokens.Length >= 3)
1077 {
1078 if (timeTriggerList == null)
1079 {
1080 timeTriggerList = new List<KeyValuePair<int, string[]>>();
1081 }
1082
1083 string[] items = new string[tokens.Length - 2];
1084 Array.Copy(tokens, 2, items, 0, items.Length);
1085 int ticks = (int)(Double.Parse(tokens[1]) * SECOND) + clock;
1086 timeTriggerList.Add(new KeyValuePair<int, string[]>(ticks, items));
1087 }
1088 else if (cmdToken.StartsWith("SAV") && tokens.Length >= 3)
1089 {
1090 char opCode = (cmdToken.Length >= 4 ? cmdToken[3] : 'B');
1091 List<IMyTerminalBlock> triggerBlocks = null;
1092 switch (opCode)
1093 {
1094 case 'B':
1095 triggerBlocks = GetBlocksWithName<IMyTerminalBlock>(tokens[1], 3);
1096 break;
1097 case 'P':
1098 triggerBlocks = GetBlocksWithName<IMyTerminalBlock>(tokens[1], 1);
1099 break;
1100 case 'S':
1101 triggerBlocks = GetBlocksWithName<IMyTerminalBlock>(tokens[1], 2);
1102 break;
1103 case 'W':
1104 triggerBlocks = GetBlocksWithName<IMyTerminalBlock>(tokens[1], 0);
1105 break;
1106 }
1107
1108 if (triggerBlocks != null)
1109 {
1110 if (savedBlockList == null)
1111 {
1112 savedBlockList = new Dictionary<string, List<IMyTerminalBlock>>();
1113 }
1114 savedBlockList[tokens[2]] = triggerBlocks;
1115 }
1116 }
1117 else if (cmdToken.Equals("SPIN") && tokens.Length >= 1)
1118 {
1119 gyroControl.SetGyroRoll(tokens.Length >= 2 ? Int32.Parse(tokens[1]) : 30);
1120 spinActive = true;
1121 }
1122 else if (cmdToken.Equals("ZTH"))
1123 {
1124 DisableAllThrusters();
1125 }
1126 else if (cmdToken.Equals("ZGY"))
1127 {
1128 gyroControl.ZeroTurnGyro();
1129 }
1130 else if (cmdToken.Equals("DISCONNECT"))
1131 {
1132 shipRefTargetPanel = null;
1133 }
1134 else if (cmdToken.Equals("ABORT"))
1135 {
1136 mode = 99;
1137 }
1138}
1139
1140void ProcessCustomConfiguration()
1141{
1142 CustomConfiguration cfg = new CustomConfiguration(Me);
1143 cfg.Load();
1144
1145 cfg.Get("missileDetachPortType", ref missileDetachPortType);
1146 cfg.Get("missileTrajectoryType", ref missileTrajectoryType);
1147 cfg.Get("missileBlockSameGridOnly", ref missileBlockSameGridOnly);
1148 cfg.Get("strShipRefTargetPanel", ref strShipRefTargetPanel);
1149 cfg.Get("strGyroscopesTag", ref strGyroscopesTag);
1150 cfg.Get("strThrustersTag", ref strThrustersTag);
1151 cfg.Get("strDetachPortTag", ref strDetachPortTag);
1152 cfg.Get("strDirectionRefBlockTag", ref strDirectionRefBlockTag);
1153 cfg.Get("strStatusDisplayPrefix", ref strStatusDisplayPrefix);
1154 cfg.Get("missileTravelHeight", ref missileTravelHeight);
1155 cfg.Get("missileDeployDistanceCorrection", ref missileDeployDistanceCorrection);
1156 cfg.Get("missileDeployCommand", ref missileDeployCommand);
1157 cfg.Get("MAX_FALL_SPEED", ref MAX_FALL_SPEED);
1158 cfg.Get("HEIGHT_DEAD_ZONE", ref HEIGHT_DEAD_ZONE);
1159 cfg.Get("readjustMaxFallSpeed", ref readjustMaxFallSpeed);
1160 cfg.Get("driftVectorReduction", ref driftVectorReduction);
1161 cfg.Get("launchSeconds", ref launchSeconds);
1162 cfg.Get("boolDrift", ref boolDrift);
1163 cfg.Get("boolLeadTarget", ref boolLeadTarget);
1164 cfg.Get("boolNaturalDampener", ref boolNaturalDampener);
1165 cfg.Get("AIM_P", ref AIM_P);
1166 cfg.Get("AIM_I", ref AIM_I);
1167 cfg.Get("AIM_D", ref AIM_D);
1168 cfg.Get("AIM_LIMIT", ref AIM_LIMIT);
1169 cfg.Get("INTEGRAL_WINDUP_UPPER_LIMIT", ref INTEGRAL_WINDUP_UPPER_LIMIT);
1170 cfg.Get("INTEGRAL_WINDUP_LOWER_LIMIT", ref INTEGRAL_WINDUP_LOWER_LIMIT);
1171 cfg.Get("MERGE_SEPARATE_WAIT_THRESHOLD", ref MERGE_SEPARATE_WAIT_THRESHOLD);
1172 cfg.Get("outputMissileStatus", ref outputMissileStatus);
1173}
1174
1175void ProcessConfigurationCommand(string commandLine)
1176{
1177 string[] keyValues = commandLine.Split(',');
1178
1179 for (int i = 0; i < keyValues.Length; i++)
1180 {
1181 string[] tokens = keyValues[i].Trim().Split(':');
1182 if (tokens.Length > 0)
1183 {
1184 ProcessSingleConfigCommand(tokens);
1185 }
1186 }
1187}
1188
1189void ProcessSingleConfigCommand(string[] tokens)
1190{
1191 string cmdToken = tokens[0].Trim();
1192 if (cmdToken.Equals("R_TAR") && tokens.Length >= 2)
1193 {
1194 strShipRefTargetPanel = tokens[1];
1195 }
1196 else if (cmdToken.Equals("V_DVR") && tokens.Length >= 2)
1197 {
1198 double dvrValue;
1199 if (Double.TryParse(tokens[1], out dvrValue))
1200 {
1201 driftVectorReduction = dvrValue;
1202 }
1203 }
1204 else if (cmdToken.Equals("V_LS") && tokens.Length >= 2)
1205 {
1206 double lsValue;
1207 if (Double.TryParse(tokens[1], out lsValue))
1208 {
1209 launchSeconds = lsValue;
1210 }
1211 }
1212 else if (cmdToken.Equals("V_MTH") && tokens.Length >= 2)
1213 {
1214 double mthValue;
1215 if (Double.TryParse(tokens[1], out mthValue))
1216 {
1217 missileTravelHeight = mthValue;
1218 }
1219 }
1220 else if (cmdToken.Equals("V_DRIFT") && tokens.Length >= 2)
1221 {
1222 bool driftValue;
1223 if (bool.TryParse(tokens[1], out driftValue))
1224 {
1225 boolDrift = driftValue;
1226 }
1227 }
1228 else if (cmdToken.Equals("V_LEAD") && tokens.Length >= 2)
1229 {
1230 bool leadValue;
1231 if (bool.TryParse(tokens[1], out leadValue))
1232 {
1233 boolLeadTarget = leadValue;
1234 }
1235 }
1236 else if (cmdToken.Equals("V_DAMP") && tokens.Length >= 2)
1237 {
1238 bool dampenerValue;
1239 if (bool.TryParse(tokens[1], out dampenerValue))
1240 {
1241 boolNaturalDampener = dampenerValue;
1242 }
1243 }
1244 else if (cmdToken.Equals("GPS") && tokens.Length > 4)
1245 {
1246 Vector3D parsedVector = new Vector3D();
1247 double result;
1248
1249 if (Double.TryParse(tokens[2], out result))
1250 {
1251 parsedVector.SetDim(0, result);
1252
1253 if (Double.TryParse(tokens[3], out result))
1254 {
1255 parsedVector.SetDim(1, result);
1256
1257 if (Double.TryParse(tokens[4], out result))
1258 {
1259 parsedVector.SetDim(2, result);
1260
1261 targetPosition = parsedVector;
1262 targetPositionSet = true;
1263 }
1264 }
1265 }
1266 }
1267}
1268
1269//------------------------------ Initialization Methods ------------------------------
1270
1271void InitLaunchingShipRefBlocks()
1272{
1273 List<IMyTerminalBlock> blocks;
1274
1275 blocks = GetBlocksWithName<IMyTextPanel>(strShipRefTargetPanel);
1276
1277 if (blocks.Count > 0)
1278 {
1279 if (blocks.Count > 1)
1280 {
1281 Echo("Warning: More than one Text Panel with name " + strShipRefTargetPanel + " found. Using first one detected.");
1282 }
1283
1284 shipRefTargetPanel = blocks[0] as IMyTextPanel;
1285 }
1286}
1287
1288bool InitMissileBlocks()
1289{
1290 List<IMyTerminalBlock> gyros = GetGyroscopes();
1291 if (gyros == null || gyros.Count == 0) return false;
1292
1293 thrusters = GetThrusters();
1294 if (thrusters == null || thrusters.Count == 0) return false;
1295
1296 remoteControl = GetRemoteControl();
1297 if (remoteControl == null) return false;
1298
1299 if (missileBlockSameGridOnly)
1300 {
1301 FilterSameGrid(Me.CubeGrid, ref gyros);
1302 FilterSameGrid(Me.CubeGrid, ref thrusters);
1303 }
1304
1305 bool isFixedDirection = false;
1306
1307 if (strDirectionRefBlockTag != null && strDirectionRefBlockTag.Length > 0)
1308 {
1309 refForwardBlock = GetSingleBlockWithName(strDirectionRefBlockTag, missileBlockSameGridOnly);
1310 isFixedDirection = (refForwardBlock != null);
1311 }
1312
1313 if (refForwardBlock == null || boolNaturalDampener == null || boolDrift == null)
1314 {
1315 thrustValues = ComputeMaxThrustValues(thrusters);
1316 }
1317
1318 if (refForwardBlock == null)
1319 {
1320 refForwardBlock = ComputeHighestThrustReference(thrusters, thrustValues);
1321 refForwardReverse = true;
1322 }
1323
1324 refWorldMatrix = refForwardBlock.WorldMatrix;
1325 if (refForwardReverse)
1326 {
1327 refWorldMatrix.Forward = refWorldMatrix.Backward;
1328 refWorldMatrix.Left = refWorldMatrix.Right;
1329 }
1330
1331 gyroControl = new GyroControl(gyros, ref refWorldMatrix);
1332
1333 InitThrusters(isFixedDirection);
1334 thrustValues = null;
1335
1336 InitPIDControllers();
1337
1338 if (boolLeadTarget == null)
1339 {
1340 boolLeadTarget = true;
1341 }
1342
1343 if (strStatusDisplayPrefix != null && strStatusDisplayPrefix.Length > 0)
1344 {
1345 List<IMyTerminalBlock> blocks = GetBlocksWithName<IMyTerminalBlock>(strStatusDisplayPrefix, 1);
1346 if (blocks.Count > 0)
1347 {
1348 statusDisplay = blocks[0];
1349
1350 if (statusDisplay.HasAction("OnOff_On"))
1351 {
1352 statusDisplay.ApplyAction("OnOff_On");
1353
1354 IMyRadioAntenna radioAntenna = statusDisplay as IMyRadioAntenna;
1355 if (radioAntenna != null && !radioAntenna.IsBroadcasting)
1356 {
1357 radioAntenna.ApplyAction("EnableBroadCast");
1358 }
1359 }
1360 }
1361 }
1362
1363 return true;
1364}
1365
1366List<IMyTerminalBlock> GetGyroscopes()
1367{
1368 List<IMyTerminalBlock> blocks = GetBlocksWithName<IMyGyro>(strGyroscopesTag);
1369 if (blocks.Count > 0)
1370 {
1371 return blocks;
1372 }
1373
1374 GridTerminalSystem.GetBlocksOfType<IMyGyro>(blocks);
1375 if (blocks.Count == 0)
1376 {
1377 Echo("Error: Missing Gyroscopes.");
1378 }
1379 return blocks;
1380}
1381
1382List<IMyTerminalBlock> GetThrusters()
1383{
1384 List<IMyTerminalBlock> blocks = GetBlocksWithName<IMyThrust>(strThrustersTag);
1385 if (blocks.Count > 0)
1386 {
1387 return blocks;
1388 }
1389
1390 GridTerminalSystem.GetBlocksOfType<IMyThrust>(blocks);
1391 if (blocks.Count == 0)
1392 {
1393 Echo("Warning: Missing Thrusters.");
1394 }
1395 return blocks;
1396}
1397
1398IMyShipController GetRemoteControl()
1399{
1400 List<IMyTerminalBlock> blocks = GetBlocksOfType<IMyShipController>();
1401 if (missileBlockSameGridOnly)
1402 {
1403 FilterSameGrid(Me.CubeGrid, ref blocks);
1404 }
1405
1406 IMyShipController remoteBlock = (blocks.Count > 0 ? blocks[0] as IMyShipController : null);
1407 if (remoteBlock == null)
1408 {
1409 Echo("Error: Missing Remote Control.");
1410 }
1411 return remoteBlock;
1412}
1413
1414void InitPIDControllers()
1415{
1416 //---------- Setup PID Controller ----------
1417
1418 if (AIM_P + AIM_I + AIM_D < 0.001)
1419 {
1420 if (Me.CubeGrid.ToString().Contains("Large"))
1421 {
1422 AIM_P = DEF_BIG_GRID_P;
1423 AIM_I = DEF_BIG_GRID_I;
1424 AIM_D = DEF_BIG_GRID_D;
1425 }
1426 else
1427 {
1428 AIM_P = DEF_SMALL_GRID_P;
1429 AIM_I = DEF_SMALL_GRID_I;
1430 AIM_D = DEF_SMALL_GRID_D;
1431 AIM_LIMIT *= 2;
1432 }
1433 }
1434
1435 yawController = new PIDController(AIM_P, AIM_I, AIM_D, INTEGRAL_WINDUP_UPPER_LIMIT, INTEGRAL_WINDUP_LOWER_LIMIT, SECOND);
1436 pitchController = new PIDController(AIM_P, AIM_I, AIM_D, INTEGRAL_WINDUP_UPPER_LIMIT, INTEGRAL_WINDUP_LOWER_LIMIT, SECOND);
1437 rollController = new PIDController(AIM_P, AIM_I, AIM_D, INTEGRAL_WINDUP_UPPER_LIMIT, INTEGRAL_WINDUP_LOWER_LIMIT, SECOND);
1438}
1439
1440void InitThrusters(bool isFixedDirection)
1441{
1442 //---------- Find Forward Thrusters ----------
1443
1444 List<IMyTerminalBlock> checkThrusters = thrusters;
1445 thrusters = new List<IMyTerminalBlock>();
1446
1447 if (!isFixedDirection || boolNaturalDampener == null || boolDrift == null)
1448 {
1449 IMyTerminalBlock leftThruster = null;
1450 IMyTerminalBlock rightThruster = null;
1451 IMyTerminalBlock upThruster = null;
1452 IMyTerminalBlock downThruster = null;
1453
1454 float leftThrustTotal = 0;
1455 float rightThrustTotal = 0;
1456 float upThrustTotal = 0;
1457 float downThrustTotal = 0;
1458
1459 for (int i = 0; i < checkThrusters.Count; i++)
1460 {
1461 Base6Directions.Direction thrusterDirection = refWorldMatrix.GetClosestDirection(checkThrusters[i].WorldMatrix.Backward);
1462 switch (thrusterDirection)
1463 {
1464 case Base6Directions.Direction.Forward:
1465 thrusters.Add(checkThrusters[i]);
1466 break;
1467 case Base6Directions.Direction.Left:
1468 leftThruster = checkThrusters[i];
1469 leftThrustTotal += thrustValues[i];
1470 break;
1471 case Base6Directions.Direction.Right:
1472 rightThruster = checkThrusters[i];
1473 rightThrustTotal += thrustValues[i];
1474 break;
1475 case Base6Directions.Direction.Up:
1476 upThruster = checkThrusters[i];
1477 upThrustTotal += thrustValues[i];
1478 if (isFixedDirection)
1479 {
1480 refDownwardBlock = upThruster;
1481 }
1482 break;
1483 case Base6Directions.Direction.Down:
1484 downThruster = checkThrusters[i];
1485 downThrustTotal += thrustValues[i];
1486 break;
1487 }
1488
1489 checkThrusters[i].ApplyAction("OnOff_On");
1490 }
1491
1492 float highestThrust = Math.Max(Math.Max(Math.Max(leftThrustTotal, rightThrustTotal), upThrustTotal), downThrustTotal);
1493 if (highestThrust == 0)
1494 {
1495 if (boolNaturalDampener == true)
1496 {
1497 Echo("Warning: Natural Gravity Dampener feature not possible as there are no Downward Thrusters found.");
1498 }
1499 boolNaturalDampener = false;
1500
1501 if (boolDrift == null)
1502 {
1503 boolDrift = true;
1504 }
1505 }
1506 else
1507 {
1508 if (!isFixedDirection)
1509 {
1510 if (leftThrustTotal == highestThrust)
1511 {
1512 refDownwardBlock = leftThruster;
1513 }
1514 else if (rightThrustTotal == highestThrust)
1515 {
1516 refDownwardBlock = rightThruster;
1517 }
1518 else if (upThrustTotal == highestThrust)
1519 {
1520 refDownwardBlock = upThruster;
1521 }
1522 else
1523 {
1524 refDownwardBlock = downThruster;
1525 }
1526 }
1527 boolNaturalDampener = (refDownwardBlock != null);
1528
1529 if (boolDrift == null)
1530 {
1531 float lowestThrust = Math.Min(Math.Min(Math.Min(leftThrustTotal, rightThrustTotal), upThrustTotal), downThrustTotal);
1532 boolDrift = (highestThrust > lowestThrust * 2);
1533 }
1534 }
1535 }
1536 else
1537 {
1538 for (int i = 0; i < checkThrusters.Count; i++)
1539 {
1540 Base6Directions.Direction thrusterDirection = refWorldMatrix.GetClosestDirection(checkThrusters[i].WorldMatrix.Backward);
1541 if (thrusterDirection == Base6Directions.Direction.Forward)
1542 {
1543 thrusters.Add(checkThrusters[i]);
1544 }
1545 else if (boolNaturalDampener == true && thrusterDirection == Base6Directions.Direction.Up)
1546 {
1547 refDownwardBlock = checkThrusters[i];
1548 }
1549
1550 checkThrusters[i].ApplyAction("OnOff_On");
1551 }
1552
1553 if (boolNaturalDampener == true && refDownwardBlock == null)
1554 {
1555 Echo("Warning: Natural Gravity Dampener feature not possible as there are no Downward Thrusters found.");
1556 boolNaturalDampener = false;
1557 }
1558 }
1559}
1560
1561float[] ComputeMaxThrustValues(List<IMyTerminalBlock> checkThrusters)
1562{
1563 float[] thrustValues = new float[checkThrusters.Count];
1564
1565 for (int i = 0; i < checkThrusters.Count; i++)
1566 {
1567 thrustValues[i] = Math.Max(((IMyThrust)checkThrusters[i]).MaxEffectiveThrust, 0.00001f);
1568 }
1569
1570 return thrustValues;
1571}
1572
1573IMyTerminalBlock ComputeHighestThrustReference(List<IMyTerminalBlock> checkThrusters, float[] thrustValues)
1574{
1575 if (checkThrusters.Count == 0)
1576 {
1577 return null;
1578 }
1579
1580 IMyTerminalBlock fwdThruster = null;
1581 IMyTerminalBlock bwdThruster = null;
1582 IMyTerminalBlock leftThruster = null;
1583 IMyTerminalBlock rightThruster = null;
1584 IMyTerminalBlock upThruster = null;
1585 IMyTerminalBlock downThruster = null;
1586
1587 float fwdThrustTotal = 0;
1588 float bwdThrustTotal = 0;
1589 float leftThrustTotal = 0;
1590 float rightThrustTotal = 0;
1591 float upThrustTotal = 0;
1592 float downThrustTotal = 0;
1593
1594 for (int i = 0; i < checkThrusters.Count; i++)
1595 {
1596 Base6Directions.Direction thrusterDirection = Me.WorldMatrix.GetClosestDirection(checkThrusters[i].WorldMatrix.Backward);
1597 switch (thrusterDirection)
1598 {
1599 case Base6Directions.Direction.Forward:
1600 fwdThruster = checkThrusters[i];
1601 fwdThrustTotal += thrustValues[i];
1602 break;
1603 case Base6Directions.Direction.Backward:
1604 bwdThruster = checkThrusters[i];
1605 bwdThrustTotal += thrustValues[i];
1606 break;
1607 case Base6Directions.Direction.Left:
1608 leftThruster = checkThrusters[i];
1609 leftThrustTotal += thrustValues[i];
1610 break;
1611 case Base6Directions.Direction.Right:
1612 rightThruster = checkThrusters[i];
1613 rightThrustTotal += thrustValues[i];
1614 break;
1615 case Base6Directions.Direction.Up:
1616 upThruster = checkThrusters[i];
1617 upThrustTotal += thrustValues[i];
1618 break;
1619 case Base6Directions.Direction.Down:
1620 downThruster = checkThrusters[i];
1621 downThrustTotal += thrustValues[i];
1622 break;
1623 }
1624 }
1625
1626 List<IMyTerminalBlock> highestThrustReferences = new List<IMyTerminalBlock>(2);
1627
1628 float highestThrust = Math.Max(Math.Max(Math.Max(Math.Max(Math.Max(fwdThrustTotal, bwdThrustTotal), leftThrustTotal), rightThrustTotal), upThrustTotal), downThrustTotal);
1629 if (fwdThrustTotal == highestThrust && fwdThruster != null)
1630 {
1631 highestThrustReferences.Add(fwdThruster);
1632 }
1633 if (bwdThrustTotal == highestThrust && bwdThruster != null)
1634 {
1635 highestThrustReferences.Add(bwdThruster);
1636 }
1637 if (leftThrustTotal == highestThrust && leftThruster != null)
1638 {
1639 highestThrustReferences.Add(leftThruster);
1640 }
1641 if (rightThrustTotal == highestThrust && rightThruster != null)
1642 {
1643 highestThrustReferences.Add(rightThruster);
1644 }
1645 if (upThrustTotal == highestThrust && upThruster != null)
1646 {
1647 highestThrustReferences.Add(upThruster);
1648 }
1649 if (downThrustTotal == highestThrust && downThruster != null)
1650 {
1651 highestThrustReferences.Add(downThruster);
1652 }
1653
1654 if (highestThrustReferences.Count == 1)
1655 {
1656 return highestThrustReferences[0];
1657 }
1658 else
1659 {
1660 Vector3D diagonalVector = ComputeBlockGridDiagonalVector(Me);
1661
1662 IMyTerminalBlock closestToLengthRef = highestThrustReferences[0];
1663 double closestToLengthValue = 0;
1664
1665 for (int i = 0; i < highestThrustReferences.Count; i++)
1666 {
1667 double dotLength = Math.Abs(diagonalVector.Dot(highestThrustReferences[i].WorldMatrix.Forward));
1668 if (dotLength > closestToLengthValue)
1669 {
1670 closestToLengthValue = dotLength;
1671 closestToLengthRef = highestThrustReferences[i];
1672 }
1673 }
1674
1675 return closestToLengthRef;
1676 }
1677}
1678
1679Vector3D ComputeBlockGridDiagonalVector(IMyTerminalBlock block)
1680{
1681 IMyCubeGrid cubeGrid = block.CubeGrid;
1682
1683 Vector3D minVector = cubeGrid.GridIntegerToWorld(cubeGrid.Min);
1684 Vector3D maxVector = cubeGrid.GridIntegerToWorld(cubeGrid.Max);
1685
1686 return (minVector - maxVector);
1687}
1688
1689//------------------------------ Thruster Control Methods ------------------------------
1690
1691void FireThrusters()
1692{
1693 if (thrusters != null)
1694 {
1695 for (int i = 0; i < thrusters.Count; i++)
1696 {
1697 thrusters[i].SetValue<float>("Override", thrusters[i].GetMaximum<float>("Override"));
1698 }
1699 }
1700}
1701
1702void DisableAllThrusters()
1703{
1704 List<IMyTerminalBlock> blocks = new List<IMyTerminalBlock>();
1705 GridTerminalSystem.GetBlocksOfType<IMyThrust>(blocks);
1706
1707 for (int i = 0; i < blocks.Count; i++)
1708 {
1709 blocks[i].ApplyAction("OnOff_Off");
1710 }
1711}
1712
1713//------------------------------ Name Finder API ------------------------------
1714
1715IMyTerminalBlock GetSingleBlockWithName(string name, bool sameGridOnly = false)
1716{
1717 List<IMyTerminalBlock> blocks = new List<IMyTerminalBlock>();
1718 GridTerminalSystem.SearchBlocksOfName(name, blocks);
1719 if (sameGridOnly)
1720 {
1721 FilterSameGrid(Me.CubeGrid, ref blocks);
1722 }
1723
1724 return (blocks.Count > 0 ? blocks[0] : null);
1725}
1726
1727List<IMyTerminalBlock> GetBlocksOfType<T>() where T: class, IMyTerminalBlock
1728{
1729 List<IMyTerminalBlock> blocks = new List<IMyTerminalBlock>();
1730 GridTerminalSystem.GetBlocksOfType<T>(blocks);
1731
1732 return blocks;
1733}
1734
1735List<T> GetBlocksOfTypeCasted<T>() where T: class, IMyTerminalBlock
1736{
1737 List<T> blocks = new List<T>();
1738 GridTerminalSystem.GetBlocksOfType<T>(blocks);
1739
1740 return blocks;
1741}
1742
1743List<IMyTerminalBlock> GetBlocksWithName(string name)
1744{
1745 List<IMyTerminalBlock> blocks = new List<IMyTerminalBlock>();
1746 GridTerminalSystem.SearchBlocksOfName(name, blocks);
1747
1748 return blocks;
1749}
1750
1751List<IMyTerminalBlock> GetBlocksWithName<T>(string name, int matchType = 0) where T: class, IMyTerminalBlock
1752{
1753 List<IMyTerminalBlock> blocks = new List<IMyTerminalBlock>();
1754 GridTerminalSystem.SearchBlocksOfName(name, blocks);
1755
1756 List<IMyTerminalBlock> filteredBlocks = new List<IMyTerminalBlock>();
1757 for (int i = 0; i < blocks.Count; i++)
1758 {
1759 if (matchType > 0)
1760 {
1761 bool isMatch = false;
1762
1763 switch (matchType)
1764 {
1765 case 1:
1766 if (blocks[i].CustomName.StartsWith(name, StringComparison.OrdinalIgnoreCase))
1767 {
1768 isMatch = true;
1769 }
1770 break;
1771 case 2:
1772 if (blocks[i].CustomName.EndsWith(name, StringComparison.OrdinalIgnoreCase))
1773 {
1774 isMatch = true;
1775 }
1776 break;
1777 case 3:
1778 if (blocks[i].CustomName.Equals(name, StringComparison.OrdinalIgnoreCase))
1779 {
1780 isMatch = true;
1781 }
1782 break;
1783 default:
1784 isMatch = true;
1785 break;
1786 }
1787
1788 if (!isMatch)
1789 {
1790 continue;
1791 }
1792 }
1793
1794 IMyTerminalBlock block = blocks[i] as T;
1795 if (block != null)
1796 {
1797 filteredBlocks.Add(block);
1798 }
1799 }
1800
1801 return filteredBlocks;
1802}
1803
1804void FilterSameGrid<T>(IMyCubeGrid grid, ref List<T> blocks) where T: class, IMyTerminalBlock
1805{
1806 List<T> filtered = new List<T>();
1807 for (int i = 0; i < blocks.Count; i++)
1808 {
1809 if (blocks[i].CubeGrid == grid)
1810 {
1811 filtered.Add(blocks[i]);
1812 }
1813 }
1814 blocks = filtered;
1815}
1816
1817public class GyroControl
1818{
1819 string[] profiles = {"Yaw","Yaw","Pitch","Pitch","Roll","Roll"};
1820
1821 List<IMyGyro> gyros;
1822
1823 private byte[] gyroYaw;
1824 private byte[] gyroPitch;
1825 private byte[] gyroRoll;
1826
1827 public GyroControl(List<IMyTerminalBlock> newGyros, ref MatrixD refWorldMatrix)
1828 {
1829 gyros = new List<IMyGyro>(newGyros.Count);
1830
1831 gyroYaw = new byte[newGyros.Count];
1832 gyroPitch = new byte[newGyros.Count];
1833 gyroRoll = new byte[newGyros.Count];
1834
1835 int index = 0;
1836 foreach (IMyTerminalBlock block in newGyros)
1837 {
1838 IMyGyro gyro = block as IMyGyro;
1839 if (gyro != null)
1840 {
1841 gyroYaw[index] = SetRelativeDirection(gyro.WorldMatrix.GetClosestDirection(refWorldMatrix.Up));
1842 gyroPitch[index] = SetRelativeDirection(gyro.WorldMatrix.GetClosestDirection(refWorldMatrix.Left));
1843 gyroRoll[index] = SetRelativeDirection(gyro.WorldMatrix.GetClosestDirection(refWorldMatrix.Forward));
1844
1845 gyros.Add(gyro);
1846
1847 index++;
1848 }
1849 }
1850
1851 }
1852
1853 public byte SetRelativeDirection(Base6Directions.Direction dir)
1854 {
1855 switch (dir)
1856 {
1857 case Base6Directions.Direction.Up:
1858 return 0;
1859 case Base6Directions.Direction.Down:
1860 return 1;
1861 case Base6Directions.Direction.Left:
1862 return 2;
1863 case Base6Directions.Direction.Right:
1864 return 3;
1865 case Base6Directions.Direction.Forward:
1866 return 5;
1867 case Base6Directions.Direction.Backward:
1868 return 4;
1869 }
1870 return 0;
1871 }
1872
1873 public void ApplyAction(string actionName)
1874 {
1875 foreach (IMyGyro gyro in gyros)
1876 {
1877 gyro.ApplyAction(actionName);
1878 }
1879 }
1880
1881 public void Enabled(bool enabled)
1882 {
1883 foreach (IMyGyro gyro in gyros)
1884 {
1885 gyro.Enabled = enabled;
1886 }
1887 }
1888
1889 public void SetGyroOverride(bool bOverride)
1890 {
1891 foreach (IMyGyro gyro in gyros)
1892 {
1893 if (gyro.GyroOverride != bOverride)
1894 {
1895 gyro.ApplyAction("Override");
1896 }
1897 }
1898 }
1899
1900 public void SetGyroYaw(float yawRate)
1901 {
1902 for (int i = 0; i < gyros.Count; i++)
1903 {
1904 byte index = gyroYaw[i];
1905 gyros[i].SetValue(profiles[index], (index % 2 == 0 ? yawRate : -yawRate) * MathHelper.RadiansPerSecondToRPM);
1906 }
1907 }
1908
1909 public void SetGyroPitch(float pitchRate)
1910 {
1911 for (int i = 0; i < gyros.Count; i++)
1912 {
1913 byte index = gyroPitch[i];
1914 gyros[i].SetValue(profiles[index], (index % 2 == 0 ? pitchRate : -pitchRate) * MathHelper.RadiansPerSecondToRPM);
1915 }
1916 }
1917
1918 public void SetGyroRoll(float rollRate)
1919 {
1920 for (int i = 0; i < gyros.Count; i++)
1921 {
1922 byte index = gyroRoll[i];
1923 gyros[i].SetValue(profiles[index], (index % 2 == 0 ? rollRate : -rollRate) * MathHelper.RadiansPerSecondToRPM);
1924 }
1925 }
1926
1927 public void ZeroTurnGyro()
1928 {
1929 for (int i = 0; i < gyros.Count; i++)
1930 {
1931 gyros[i].SetValue(profiles[gyroYaw[i]], 0f);
1932 gyros[i].SetValue(profiles[gyroPitch[i]], 0f);
1933 }
1934 }
1935
1936 public void ResetGyro()
1937 {
1938 foreach (IMyGyro gyro in gyros)
1939 {
1940 gyro.SetValue("Yaw", 0f);
1941 gyro.SetValue("Pitch", 0f);
1942 gyro.SetValue("Roll", 0f);
1943 }
1944 }
1945}
1946
1947public class PIDController
1948{
1949 double integral;
1950 double lastInput;
1951
1952 double gain_p;
1953 double gain_i;
1954 double gain_d;
1955 double upperLimit_i;
1956 double lowerLimit_i;
1957 double second;
1958
1959 public PIDController(double pGain, double iGain, double dGain, double iUpperLimit = 0, double iLowerLimit = 0, float stepsPerSecond = 60f)
1960 {
1961 gain_p = pGain;
1962 gain_i = iGain;
1963 gain_d = dGain;
1964 upperLimit_i = iUpperLimit;
1965 lowerLimit_i = iLowerLimit;
1966 second = stepsPerSecond;
1967 }
1968
1969 public double Filter(double input, int round_d_digits)
1970 {
1971 double roundedInput = Math.Round(input, round_d_digits);
1972
1973 integral = integral + (input / second);
1974 integral = (upperLimit_i > 0 && integral > upperLimit_i ? upperLimit_i : integral);
1975 integral = (lowerLimit_i < 0 && integral < lowerLimit_i ? lowerLimit_i : integral);
1976
1977 double derivative = (roundedInput - lastInput) * second;
1978 lastInput = roundedInput;
1979
1980 return (gain_p * input) + (gain_i * integral) + (gain_d * derivative);
1981 }
1982
1983 public void Reset()
1984 {
1985 integral = lastInput = 0;
1986 }
1987}
1988
1989public class CustomConfiguration
1990{
1991 public IMyTerminalBlock configBlock;
1992 public Dictionary<string, string> config;
1993
1994 public CustomConfiguration(IMyTerminalBlock block)
1995 {
1996 configBlock = block;
1997 config = new Dictionary<string, string>(StringComparer.OrdinalIgnoreCase);
1998 }
1999
2000 public void Load()
2001 {
2002 ParseCustomData(configBlock, config);
2003 }
2004
2005 public void Save()
2006 {
2007 WriteCustomData(configBlock, config);
2008 }
2009
2010 public string Get(string key, string defVal = null)
2011 {
2012 return config.GetValueOrDefault(key.Trim(), defVal);
2013 }
2014
2015 public void Get(string key, ref string res)
2016 {
2017 string val;
2018 if (config.TryGetValue(key.Trim(), out val))
2019 {
2020 res = val;
2021 }
2022 }
2023
2024 public void Get(string key, ref int res)
2025 {
2026 int val;
2027 if (int.TryParse(Get(key), out val))
2028 {
2029 res = val;
2030 }
2031 }
2032
2033 public void Get(string key, ref float res)
2034 {
2035 float val;
2036 if (float.TryParse(Get(key), out val))
2037 {
2038 res = val;
2039 }
2040 }
2041
2042 public void Get(string key, ref double res)
2043 {
2044 double val;
2045 if (double.TryParse(Get(key), out val))
2046 {
2047 res = val;
2048 }
2049 }
2050
2051 public void Get(string key, ref bool res)
2052 {
2053 bool val;
2054 if (bool.TryParse(Get(key), out val))
2055 {
2056 res = val;
2057 }
2058 }
2059 public void Get(string key, ref bool? res)
2060 {
2061 bool val;
2062 if (bool.TryParse(Get(key), out val))
2063 {
2064 res = val;
2065 }
2066 }
2067
2068 public void Set(string key, string value)
2069 {
2070 config[key.Trim()] = value;
2071 }
2072
2073 public static void ParseCustomData(IMyTerminalBlock block, Dictionary<string, string> cfg, bool clr = true)
2074 {
2075 if (clr)
2076 {
2077 cfg.Clear();
2078 }
2079
2080 string[] arr = block.CustomData.Split(new char[] {'\r','\n'}, StringSplitOptions.RemoveEmptyEntries);
2081 for (int i = 0; i < arr.Length; i++)
2082 {
2083 string ln = arr[i];
2084 string va;
2085
2086 int p = ln.IndexOf('=');
2087 if (p > -1)
2088 {
2089 va = ln.Substring(p + 1);
2090 ln = ln.Substring(0, p);
2091 }
2092 else
2093 {
2094 va = "";
2095 }
2096 cfg[ln.Trim()] = va.Trim();
2097 }
2098 }
2099
2100 public static void WriteCustomData(IMyTerminalBlock block, Dictionary<string, string> cfg)
2101 {
2102 StringBuilder sb = new StringBuilder(cfg.Count * 100);
2103 foreach (KeyValuePair<string, string> va in cfg)
2104 {
2105 sb.Append(va.Key).Append('=').Append(va.Value).Append('\n');
2106 }
2107 block.CustomData = sb.ToString();
2108 }
2109}