AutoPilot/Program.cs

   1 /*
   2  * API index: https://github.com/malware-dev/MDK-SE/wiki/Api-Index
   3  * Vector transformation: https://github.com/malware-dev/MDK-SE/wiki/Vector-Transformations-with-World-Matrices
   4  * How to get rotation/position: https://forum.keenswh.com/threads/how-do-i-get-the-world-position-and-rotation-of-a-ship.7363867/
   5  *
   6  */
   7
   8 using Sandbox.Game.EntityComponents;
   9 using Sandbox.ModAPI.Ingame;
  10 using Sandbox.ModAPI.Interfaces;
  11
  12 using SpaceEngineers.Game.ModAPI.Ingame;
  13
  14 using System;
  15 using System.Collections;
  16 using System.Collections.Generic;
  17 using System.Collections.Immutable;
  18 using System.Linq;
  19 using System.Text;
  20
  21 using VRage;
  22 using VRage.Collections;
  23 using VRage.Game;
  24 using VRage.Game.Components;
  25 using VRage.Game.GUI.TextPanel;
  26 using VRage.Game.ModAPI.Ingame;
  27 using VRage.Game.ModAPI.Ingame.Utilities;
  28 using VRage.Game.ObjectBuilders.Definitions;
  29
  30 using VRageMath;
  31 using VRageMath.Spatial;
  32
  33 namespace IngameScript
  34 {
  35     partial class Program : MyGridProgram
  36     {
  37         const int CONSOLE_NB_LINES = 9;
  38         const float MAX_SPEED = 0.15f; // In rad per second.
  39         const int NB_OF_GYRO = 1000; // To limit the number of gyros set to overdrive.
  40
  41         enum State
  42         {
  43             NORMAL, // Normal state: Manual commands.
  44             AUTO_STABILIZATION,
  45             AUTO_DOCKING,
  46         }
  47
  48         State state = State.NORMAL;
  49
  50         readonly Output output;
  51
  52         IMyRemoteControl remoteController;
  53         IMyCubeGrid grid;
  54         readonly List<IMyGyro> gyros = new List<IMyGyro>();
  55
  56         IMyBroadcastListener connnectorMinerPositionListener;
  57         MatrixD connectorMinerPosition;
  58
  59         public Program()
  60         {
  61             this.grid = this.Me.CubeGrid;
  62
  63             var cockpits = new List<IMyCockpit>();
  64             this.GridTerminalSystem.GetBlocksOfType(cockpits);
  65             IMyCockpit cockpit = null;
  66             foreach (var c in cockpits)
  67             {
  68                 if (c.CubeGrid == this.grid)
  69                 {
  70                     cockpit = c;
  71                     break;
  72                 }
  73             }
  74
  75             if (cockpit == null)
  76             {
  77                 this.Echo("Can't find a cockpit");
  78                 return;
  79             }
  80
  81             this.output = new Output(cockpit, CONSOLE_NB_LINES);
  82
  83             this.output.Print("Intializing navigation control...");
  84
  85             var remoteControls = new List<IMyRemoteControl>();
  86             this.GridTerminalSystem.GetBlocksOfType(remoteControls);
  87             foreach (var rc in remoteControls)
  88             {
  89                 if (rc.CubeGrid == this.grid)
  90                 {
  91                     this.remoteController = rc;
  92                     break;
  93                 }
  94             }
  95
  96             this.GridTerminalSystem.GetBlocksOfType(
  97                 this.gyros,
  98                 (IMyGyro gyro) => gyro.CubeGrid == this.grid
  99             );
 100
 101             if (this.remoteController == null)
 102             {
 103                 this.output.Print("Can't find a remote controller");
 104                 return;
 105             }
 106
 107
 108             this.connnectorMinerPositionListener = this.IGC.RegisterBroadcastListener("POSITION_CONNECTOR_MINER");
 109
 110             /*
 111             var sensor = this.GridTerminalSystem.GetBlockWithName("Sensor") as IMySensorBlock;
 112             var entities = new List<MyDetectedEntityInfo>();
 113             sensor.DetectedEntities(entities);
 114             foreach (var entity in entities)
 115             {
 116                 entity.
 117                 this.output.Print(entity.Name);
 118             }*/
 119
 120             this.Runtime.UpdateFrequency = UpdateFrequency.Update10;
 121
 122             this.output.Print("Navigation control initializing complete");
 123         }
 124
 125         public void Save()
 126         {
 127         }
 128
 129         void UpdateStateAutoStabilization()
 130         {
 131             //var pos = this.remoteController.Position;
 132             var gravity = this.remoteController.GetNaturalGravity().Normalized();
 133
 134             if (!Vector3D.IsZero(gravity))
 135             {
 136                 var worldMatrix = this.remoteController.WorldMatrix;
 137                 var up = worldMatrix.Up;
 138                 var forward = worldMatrix.Forward;
 139                 var left = worldMatrix.Left;
 140
 141                 //this.Print(gravity.ToString());
 142                 //gravity.ToString();
 143                 this.output.Display(
 144                     $"Gravity: {gravity.ToString("f2")}\n" +
 145                     $"Up: {up.ToString("f2")}\n" +
 146                     $"Foward: {forward.ToString("f2")}\n" +
 147                     $"Left: {left.ToString("f2")}\n",
 148                     1
 149                 );
 150
 151                 var alpha = (float)Math.Asin(up.Dot(gravity));
 152                 var beta = (float)Math.Asin(left.Dot(gravity));
 153
 154                 this.output.Display(
 155                     $"Alpha: {alpha}\n" +
 156                     $"Beta: {beta}\n" +
 157                     $"Is under control: {this.remoteController.IsUnderControl}",
 158                     2
 159                 );
 160
 161                 for (int i = 0; i < NB_OF_GYRO && i < this.gyros.Count; i++)
 162                 {
 163                     var gyro = this.gyros[i];
 164                     gyro.Pitch = MathHelper.Clamp(-alpha, -MAX_SPEED, MAX_SPEED);
 165                     gyro.Roll = MathHelper.Clamp(beta, -MAX_SPEED, MAX_SPEED);
 166                 }
 167             }
 168         }
 169
 170         void UpdateStateAutoDocking()
 171         {
 172             if (this.connnectorMinerPositionListener.HasPendingMessage)
 173                 this.connectorMinerPosition = this.connnectorMinerPositionListener.AcceptMessage().As<MatrixD>();
 174
 175             if (this.connectorMinerPosition.IsValid())
 176             {
 177                 this.output.Display($"Connector position:\n{this.connectorMinerPosition}", 3);
 178             }
 179             else
 180             {
 181                 this.output.Display($"NOPE", 3);
 182             }
 183         }
 184
 185         void UpdateState()
 186         {
 187             switch (this.state)
 188             {
 189                 case State.AUTO_STABILIZATION:
 190                     this.UpdateStateAutoStabilization();
 191                     break;
 192                 case State.AUTO_DOCKING:
 193                     this.UpdateStateAutoDocking();
 194                     break;
 195                 case State.NORMAL:
 196                     break; // Nothing.
 197             }
 198         }
 199
 200         void UpdateStateMachine(State newState)
 201         {
 202             if (this.state == newState)
 203                 return;
 204
 205             if (newState == State.NORMAL)
 206             {
 207                 switch (this.state)
 208                 {
 209                     case State.AUTO_STABILIZATION:
 210                         foreach (var gyro in this.gyros)
 211                             gyro.GyroOverride = false;
 212                         this.state = State.NORMAL;
 213                         this.output.Print("Auto stabilization disabled");
 214                         break;
 215                     case State.AUTO_DOCKING:
 216                         this.state = State.NORMAL;
 217                         this.output.Print("Auto docking disabled");
 218                         break;
 219                     case State.NORMAL:
 220                         break; // Nothing.
 221
 222                 }
 223             }
 224             else
 225             {
 226                 this.UpdateStateMachine(State.NORMAL);
 227                 switch (newState)
 228                 {
 229                     case State.AUTO_STABILIZATION:
 230                         for (int i = 0; i < NB_OF_GYRO && i < this.gyros.Count; i++)
 231                         {
 232                             var gyro = this.gyros[i];
 233                             gyro.GyroOverride = true;
 234                             gyro.Yaw = 0f;
 235                             gyro.Pitch = 0f;
 236                             gyro.Roll = 0f;
 237                         }
 238
 239                         this.state = State.AUTO_STABILIZATION;
 240                         this.output.Print("Auto stabilization enabled");
 241                         break;
 242                     case State.AUTO_DOCKING:
 243                         this.state = State.AUTO_DOCKING;
 244                         this.output.Print("Auto docking enabled");
 245                         break;
 246                     case State.NORMAL:
 247                         break; // Nothing.
 248                 }
 249             }
 250         }
 251
 252         public void Main(string argument, UpdateType updateSource)
 253         {
 254             if ((updateSource & UpdateType.Update10) != 0)
 255             {
 256                 this.UpdateState();
 257             }
 258             else if ((updateSource & (UpdateType.Terminal | UpdateType.Trigger)) != 0)
 259             {
 260                 switch (argument)
 261                 {
 262                     case "SWITCH_AUTO_STABILIZATION":
 263                         if (this.state != State.AUTO_STABILIZATION)
 264                             this.UpdateStateMachine(State.AUTO_STABILIZATION);
 265                         else
 266                             this.UpdateStateMachine(State.NORMAL);
 267                         break;
 268
 269                     case "SWITCH_AUTO_DOCKING":
 270                         if (this.state != State.AUTO_DOCKING)
 271                             this.UpdateStateMachine(State.AUTO_DOCKING);
 272                         else
 273                             this.UpdateStateMachine(State.NORMAL);
 274                         break;
 275
 276                     default:
 277                         this.output.Print($"Uknown command: {argument}");
 278                         break;
 279                 }
 280             }
 281         }
 282     }
 283 }