1|   List<IMyThrust> ThrustL = new List<IMyThrust>();
    2|   List<IMyThrust> ThrustR = new List<IMyThrust>();
    3|   List<IMyThrust> ThrustU = new List<IMyThrust>();
    4|   List<IMyThrust> ThrustD = new List<IMyThrust>();
    5|   List<IMyThrust> ThrustF = new List<IMyThrust>();
    6|   List<IMyThrust> ThrustB = new List<IMyThrust>();
    7|   List<IMyThrust> Thrusts = new List<IMyThrust>();
    8|   List<IMyShipDrill> Drills = new List<IMyShipDrill>();
    9|   List<IMyGyro> gyros = new List<IMyGyro>();
  10|   IMyRemoteControl rc;
  11|   IMyTextSurfaceProvider tsp;
  12|   IMyTextSurface surface;
  13|   Vector3D srcGPS, destGPS, startGPS;
  14|   MyIni ini;
  15|   MyIniParseResult result;
  16|   string status;
  17|   int MaxSteps;
  18|   int step;
  19|   string mode;
  20|   Vector3D RefVector;
  21|   float Distance;
  22|   
  23|   public Program()
  24|   {
  25|       IMyBlockGroup group = GridTerminalSystem.GetBlockGroupWithName("ThrustersF");
  26|       group.GetBlocksOfType(ThrustF);
  27|       group = GridTerminalSystem.GetBlockGroupWithName("ThrustersB");
  28|       group.GetBlocksOfType(ThrustB);
  29|       group = GridTerminalSystem.GetBlockGroupWithName("ThrustersL");
  30|       group.GetBlocksOfType(ThrustL);
  31|       group = GridTerminalSystem.GetBlockGroupWithName("ThrustersR");
  32|       group.GetBlocksOfType(ThrustR);
  33|       group = GridTerminalSystem.GetBlockGroupWithName("ThrustersU");
  34|       group.GetBlocksOfType(ThrustU);
  35|       group = GridTerminalSystem.GetBlockGroupWithName("ThrustersD");
  36|       group.GetBlocksOfType(ThrustD);
  37|       GridTerminalSystem.GetBlocksOfType(Thrusts);
  38|       GridTerminalSystem.GetBlocksOfType(gyros);
  39|       GridTerminalSystem.GetBlocksOfType(Drills);
  40|       rc = GridTerminalSystem.GetBlockWithName("Remote Control") as IMyRemoteControl;
  41|       tsp = GridTerminalSystem.GetBlockWithName("Flight Seat") as IMyTextSurfaceProvider;
  42|       surface = tsp.GetSurface(0);
  43|       surface.ContentType = ContentType.TEXT_AND_IMAGE;
  44|       surface.Script = string.Empty;
  45|       status = "Idle";
  46|       Runtime.UpdateFrequency = UpdateFrequency.Update1;
  47|   }
  48|   
  49|   public void Main(string argument, UpdateType updateSource)
  50|   {
  51|       var command = argument.Split(',');
  52|       float angleYaw, anglePitch, angleRoll;
  53|       float TargetLoading = 0.8f;
  54|       float Kpl = 4.0f;
  55|       float DigV;
  56|   
  57|       switch (command[0]){
  58|           case "Start":
  59|               ini = new MyIni();
  60|               ini.TryParse(Me.CustomData, out result);
  61|               MaxSteps = int.Parse(ini.Get("Setting", "Steps").ToString());
  62|               step = 1;
  63|               status = "Preparation";
  64|               break;
  65|           case "End":
  66|               status = "Idle";
  67|               StopSystem();
  68|               break;
  69|       }
  70|   
  71|       switch (status){
  72|           case "Idle":
  73|               break;
  74|           case "Preparation":
  75|               mode = ini.Get(step.ToString(), "mode").ToString();
  76|               var startP = ini.Get(step.ToString(), "start_point").ToString().Split(',');
  77|               var endP = ini.Get(step.ToString(), "end_point").ToString().Split(',');
  78|               var refV = ini.Get(step.ToString(), "RefVector").ToString().Split(',');
  79|               srcGPS = new Vector3D(double.Parse(startP[0]), double.Parse(startP[1]), double.Parse(startP[2]));
  80|               destGPS = new Vector3D(double.Parse(endP[0]), double.Parse(endP[1]), double.Parse(endP[2]));
  81|               RefVector = new Vector3D(double.Parse(refV[0]), double.Parse(refV[1]), double.Parse(refV[2]));
  82|               status = "MoveToSrc";
  83|               startGPS = rc.GetPosition();
  84|               foreach (var drill in Drills)
  85|                   drill.ApplyAction("OnOff_On");
  86|               break;
  87|           case "MoveToSrc":
  88|               angleYaw = Math.Abs(CorrectYaw(destGPS-srcGPS));
  89|               anglePitch = Math.Abs(CorrectPitch(destGPS-srcGPS));
  90|               angleRoll = Math.Abs(CorrectRoll(RefVector));
  91|               Distance = Math.Abs(MoveTo(2.0f, startGPS, srcGPS));
  92|               if (angleYaw < 0.01 & anglePitch < 0.01 & angleRoll < 0.01 & Distance < 0.03)
  93|                   status = "MoveToDest";
  94|               break;
  95|           case "MoveToDest":
  96|               DigV = Kpl*(TargetLoading - GetLoading());
  97|               Distance = Math.Abs(MoveTo(DigV, srcGPS, destGPS));
  98|               if (Distance < 0.03){
  99|                   status = mode == "Return" ? "Return" : "Next";
100|                   foreach (var drill in Drills)
101|                       drill.ApplyAction("OnOff_Off");
102|               }
103|               CorrectRoll(RefVector);
104|               CorrectYaw(destGPS-srcGPS);
105|               CorrectPitch(destGPS-srcGPS);
106|               break;
107|           case "Return":
108|               Distance = Math.Abs(MoveTo(2.0f, destGPS, srcGPS));
109|               status = Distance < 0.03 ? "Next" : "Return";
110|               CorrectRoll(RefVector);
111|               CorrectYaw(destGPS-srcGPS);
112|               CorrectPitch(destGPS-srcGPS);
113|               break;
114|           case "Next":
115|               if (MaxSteps == step){
116|                   StopSystem();
117|                   status = "Idle";
118|               }
119|               else{
120|                   step++;
121|                   status = "Preparation";
122|               }
123|               break;
124|       }    
125|       UpdateDisplay();
126|   }
127|   
128|   float MoveTo(float Speed_Limit, Vector3D Src_Pos, Vector3D Dest_Pos)
129|   {
130|       Vector3D DestV = Dest_Pos - rc.GetPosition();
131|       Vector3D CurVelocity = rc.GetShipVelocities().LinearVelocity;
132|       Vector3D TargetVelocity = Speed_Limit*DestV/DestV.Length();
133|       Vector3D CorrV = TargetVelocity - CurVelocity;
134|       Vector3D SD = Dest_Pos - Src_Pos;
135|       Vector3D SA = rc.GetPosition() - Src_Pos;
136|       Vector3D n = Vector3D.Cross(SD, SA);
137|       Vector3D P = Vector3D.Cross(SD, n);
138|       double theta = Math.Acos(Vector3D.Dot(SA, SD)/(SA.Length()*SD.Length()));
139|       P = P*Math.Sin(theta)*SA.Length()/P.Length();
140|       double mass = rc.CalculateShipMass().TotalMass;
141|       double Kp = 0.8;
142|       double Kpc = 0.5;
143|      
144|       Vector3D a = -rc.GetTotalGravity()  + CorrV * Kp + P * Kpc;
145|       
146|       double f = Vector3D.Dot(a, rc.WorldMatrix.Forward);
147|       double l = Vector3D.Dot(a, rc.WorldMatrix.Left);
148|       double u = Vector3D.Dot(a, rc.WorldMatrix.Up);
149|   
150|       foreach (var Thrust in ThrustF)
151|           Thrust.ThrustOverride = f > 0 ? (Single)(f * mass / ThrustF.Count) : 0f;
152|       foreach (var Thrust in ThrustB)
153|           Thrust.ThrustOverride = f < 0 ? (Single)(-f * mass / ThrustB.Count) : 0f;
154|   
155|       foreach (var Thrust in ThrustL)
156|           Thrust.ThrustOverride = l > 0 ? (Single)(l * mass / ThrustL.Count) : 0f;
157|       foreach (var Thrust in ThrustR)
158|           Thrust.ThrustOverride = l < 0 ? (Single)(-l * mass / ThrustR.Count) : 0f;
159|   
160|       foreach (var Thrust in ThrustU)
161|           Thrust.ThrustOverride = u > 0 ? (Single)(u * mass / ThrustU.Count) : 0f;
162|       foreach (var Thrust in ThrustD)
163|           Thrust.ThrustOverride = u < 0 ? (Single)(-u * mass / ThrustD.Count) : 0f;
164|   
165|       return (float)(DestV.Length());
166|   }
167|   
168|   private float CorrectPitch(Vector3D Direction)
169|   {
170|       Direction.Normalize();
171|       Vector3D TargetV = Vector3D.Cross(rc.WorldMatrix.Left, Direction);
172|       double diff_Pitch_angle = Math.Acos(Vector3D.Dot(TargetV, rc.WorldMatrix.Backward))-Math.PI/2;
173|       double scaling_factor = 5;
174|   
175|       foreach(var gyro in gyros){
176|           gyro.SetValue("Override", true);
177|           gyro.SetValue("Pitch", (Single)(scaling_factor * diff_Pitch_angle));
178|       }
179|       return (Single)(diff_Pitch_angle*180/Math.PI);
180|   }
181|   
182|   private float CorrectYaw(Vector3D Direction)
183|   {
184|       Direction.Normalize();
185|       Vector3D TargetV = Vector3D.Cross(rc.WorldMatrix.Up, Direction);
186|       double diff_Yaw_angle = Math.Acos(Vector3D.Dot(TargetV, rc.WorldMatrix.Backward))-Math.PI/2;
187|       double scaling_factor = 5;
188|   
189|       foreach(var gyro in gyros){
190|           gyro.SetValue("Override", true);
191|           gyro.SetValue("Yaw", (Single)(scaling_factor * diff_Yaw_angle));
192|       }
193|       return (Single)(diff_Yaw_angle*180/Math.PI);
194|   }
195|   
196|   private float CorrectRoll(Vector3D Ref)
197|   {
198|       double scaling_factor = 5;    
199|       Ref = Ref.Length() == 0 ? -rc.GetNaturalGravity() : Ref;
200|       Vector3D TargetV = Vector3D.Cross(rc.WorldMatrix.Forward, Ref);
201|       TargetV.Normalize();
202|       double diff_angle = Math.Acos(Vector3D.Dot(TargetV, rc.WorldMatrix.Up))-Math.PI/2;
203|   
204|       foreach(var gyro in gyros){
205|           gyro.SetValue("Override", true);
206|           gyro.SetValue("Roll", (Single)(scaling_factor * diff_angle));
207|       }
208|       return (Single)(diff_angle*180/Math.PI);
209|   }
210|   
211|   private float GetLoading()
212|   {
213|       float max = 0;
214|       foreach(var thrust in Thrusts)
215|           max = max < thrust.CurrentThrust ? thrust.CurrentThrust : max;
216|       return (max/Thrusts[0].MaxThrust);
217|   }
218|   
219|   private void StopSystem()
220|   {
221|       foreach (var Thrust in Thrusts)
222|           Thrust.ThrustOverride = 0f;
223|       foreach (var drill in Drills)
224|           drill.ApplyAction("OnOff_Off");
225|       foreach(var gyro in gyros){
226|           gyro.SetValue("Roll", 0f);
227|           gyro.SetValue("Yaw", 0f);
228|           gyro.SetValue("Pitch", 0f);
229|           gyro.SetValue("Override", false);
230|       }
231|   }
232|   
233|   private void UpdateDisplay()
234|   {
235|       float power_L = ThrustL[0].CurrentThrust;
236|       float power_R = ThrustR[0].CurrentThrust;
237|       float power_U = ThrustU[0].CurrentThrust;
238|       float power_D = ThrustD[0].CurrentThrust;
239|       float power_F = ThrustF[0].CurrentThrust;
240|       float power_B = ThrustB[0].CurrentThrust;
241|       float max = Thrusts[0].MaxThrust;
242|       
243|       String msg = "Status: " + status + "\n";
244|       msg += "Velocity: " + rc.GetShipSpeed().ToString("0.00 m/s\n");
245|       msg += "Total Ship Mass: " + rc.CalculateShipMass().TotalMass.ToString("0 kg\n");
246|       msg += "Loading ratio: " + GetLoading().ToString("0%\n");
247|       msg += power_U > power_D ? (power_U/max).ToString("^ 0.00 ") : (power_D/max).ToString("v 0.00 ");
248|       msg += power_L > power_R ? (power_L/max).ToString("< 0.00 ") : (power_R/max).ToString("> 0.00 ");
249|       msg += power_F > power_B ? (power_F/max).ToString("F 0.00\n") : (power_B/max).ToString("B 0.00\n");
250|       msg += "Remain: " + Distance.ToString("0.00 m\n");
251|       surface.WriteText(msg);
252|   }