自動掘削船
地下拠点開発のため、正確に岩盤を掘削できる自動掘削船を作りました。掘削船前方に 5x5 基のドリルを持ち 1 回の掘削で縦横 12.5 m の掘削穴を惑星上のどの方向にでも掘り進むことができます。掘削に必要な掘削経路の情報は、Programmable block の Custom Data に ini 形式で記述します。
掘削船は、左右方向に各 11 基、前後上下方向に各 8 基、合計 54 基の Hydrogen Thruster、25 基の Drill、2 基の Gyroscope および Remote Control、Programmable Block を各 1 基持っています。プログラム冒頭でこれらのブロックを格納する変数をグローバル変数として宣言しています。また、Flight Seat の LCD画面に掘削船の各種情報を表示するための IMyTextSurfaceProvider 型および IMyTextSurface 型の変数や、掘削経路情報を Programmable Block の Custom Data から読み込むための MyIni 型の変数も宣言しています。
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;
Main() の前半部分では、コマンドライン引数から文字列を command 変数に取り出し、文字列が "Start" の場合、Custom Data から掘削経路情報を読み込み status 変数を Preparation を設定して掘削を開始します。command 変数が "End" の場合、status を Idle にして掘削を中止します。
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| }
Main() の後半部分では、status の内容に応じて掘削船は以下の動作を実行します。
Preparation
Custom Data 中のデータから該当する掘削ステップ (変数 step) の掘削経路情報を読み込み、掘削開始座標 srcGPS、掘削終了座標 destGSP および 垂直掘削する際の姿勢制御に使用する RefVector に値をセットします。status を MoveToSrc に変更した後、ドリルを作動させます。
MoveToSrc
船首を掘削方向である destGPS に向けながら、掘削開始座標 srcGPS に向けて最大船速 2.0 m/s で移動します。移動が完了したら status を MoveToDest に変更します。
MoveToDest
掘削中の船体重量の変化に伴う Hydrogen Thruster の出力負荷が最大値を超えないように掘削速度 DigV を設定し、destGPS に向けて掘削しながら前進します。destGPS まで掘り進んだらドリルを停止して掘削を終了します。掘削経路情報の mode が Return である場合、掘り進んできた経路をそのまま srcGPS まで引き返します。mode が Next であった場合、destGPS から次の動作に移ります。
Return
srcGPS まで姿勢制御しながら、掘削してきたルートを引き返します。srcGPS まで戻ったら status を Nextに更新します。
Next
掘削ステップ step が MaxSteps に達していれば掘削動作を終了し、status を Idle に更新します。MaxSteps に達していなければ、step をインクリメントし status を Preparation に更新し、次の掘削動作に移ります。
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();
与えられた掘削開始座標 Src_Pos から終了座標 Dest_Pos までの直線上を制限速度 Speed_Limit で移動します。移動のために掘削船にかける加速度 a は、反対向きの重力ベクトルと CorrV * Kp および P * Kpc の 3 つのベクトルの和からなります。（144 行）反対向きの重力ベクトルは掘削船を惑星の重力に逆らって静止させるためのものです。CorrV は現在の掘削船の進行方向を掘削終了座標 Dest_Pos に向けるのに必要なベクトルです。3 つ目の P は掘削船の現在座標を Src_Pos と Dest_Pos を結ぶ直線上に平行移動するために必要なベクトルで、Kp と Kpc はそれぞれの係数です。このベクトル a を掘削船の前方、左方向、上方向の 3 つのベクトルに分解し（146-148 行）、その結果から上下左右前後 6 方向のそれぞれのスラスターの出力を計算して設定します（150-163 行）。最後にこの関数は終了座標 Dest_Pos までの距離を返します。
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| }
CorrectPitch、CorrectYaw および CorrectRoll はそれぞれ船体のピッチ、ヨー、ロールをオーバーライドして姿勢制御をします。通常、ロールの制御は、船体が惑星に対して水平になるように重力ベクトルを使って制御しますが、船体を垂直にした場合、ロールの制御ができなくなってしまいます。これを回避するために、Custom Data の掘削経路データに RefVector があります。この RefVector がゼロベクトルなら通常通り惑星に対して水平にロールを制御しますが、そうでない場合は、船体の前方ベクトル（WorldMatrix.Forward）と RefVector を含む平面の法線ベクトル（TargetV）を求め（200 行）、この TargetV と船体の上方ベクトル（WorldMatrix.Up）のなす角が 90° になるようにロールを制御します（202 行）。
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| }
GetLoading は（211 行）、現在最も大きな出力の Thruster が最大値の何割の力を出力しているかを返す関数です。返す値は 0 から 1 までの float 値です。
StopSystem は（219 行）、自動制御を終了し掘削船を Flight Seat から手動で操縦できるように、Thruster と Gyroscope の設定値を 0 にしてオーバーライドを解除し、Drill を停止します。
最後の UpdateDisplay は（233 行）、Flight Seat の LCD 画面上に表示されている、掘削船の現在の状況をリアルタイムで更新する関数です。
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| }