//Space Engineers Automatic Train Control Script //Written by ZephRay for ATSES /* Configurations */ public const string PROGRAM_BLOCK_NAME = "Train CU"; //编程块名称 public const string LCD_BLOCK_NAME = "Train CU LCD"; //调试LCD名称 public const string BACK_THRUSTER_NAME = "Train Back Thruster"; //后推进器名称 public const string FORWARD_THRUSTER_NAME = "Train Forward Thruster"; //前推进器名称 public const double ALLOW_VELOCITY_ERROR = 3.0; //调速控制误差范围 m public const double ALLOW_ALIGNMENT_ERROR = 5.0; // 停车对齐误差范围 m public const double ALIGNMENT_VELOCITY = 4.0; //停车对齐速度 m/s public const double MAXIMUM_VELOCITY = 40.0; //最大运行速度 m/s public const double DEACCELERATE_DISTANCE = 200.0; //减速距离 m public const float ALIGNMENT_POWER = 20; //停车对齐功率 kN public const float MAXIMUM_POWER = 400; //正常运行功率 kN public const double DOCK_TIME = 10; //站点停靠时间 public const bool DIR_FORWARD = false; public const bool DIR_BACK = true; /* Global Variables */ //Current FSM state //S0 - Alignment //S1 - Stop waiting //S2 - Accelerating //S3 - Deaccelerating int currentState; //Current running direction //DIR_FORWARD - Begin to end //DIR_BACK - End to Begin bool runningDirection; Vector3D beginPosition; Vector3D endPosition; Vector3D lastPosition; Vector3D currentPosition; double distToBegin, distToEnd, displacement; DateTime lastTime, curTime, startTime; double timeDelta; double timeToWait; double tripTime; double curVel; bool firstRun; IMyTextPanel lcdBlock; IMyThrust backThruster; IMyThrust forwardThruster; //Load parameters from Storage private void LoadStorage() { string[] fragments = Storage.Split('|'); //Do we have storage? if (fragments.Length == 2) { //Get begin position string[] coords = fragments[0].Split(','); beginPosition = new Vector3D( Math.Round(Convert.ToDouble(coords[0]), 4), Math.Round(Convert.ToDouble(coords[1]), 4), Math.Round(Convert.ToDouble(coords[2]), 4)); //Get end position coords = fragments[1].Split(','); endPosition = new Vector3D( Math.Round(Convert.ToDouble(coords[0]), 4), Math.Round(Convert.ToDouble(coords[1]), 4), Math.Round(Convert.ToDouble(coords[2]), 4)); } else { //Use default value beginPosition = new Vector3D(0, 0, 0); } } //Save parameters to Storage public void SaveStorage() { Storage = beginPosition.GetDim(0).ToString() + ',' + beginPosition.GetDim(1).ToString() + ',' + beginPosition.GetDim(2).ToString() + '|' + endPosition.GetDim(0).ToString() + ',' + endPosition.GetDim(1).ToString() + ',' + endPosition.GetDim(2).ToString(); } public void GetCurrentPosition() { currentPosition = Me.GetPosition(); } public void calcDistances() { if (runningDirection == DIR_FORWARD) { distToBegin = VRageMath.Vector3D.Distance(currentPosition, beginPosition); distToEnd = VRageMath.Vector3D.Distance(currentPosition, endPosition); } else { distToEnd = VRageMath.Vector3D.Distance(currentPosition, beginPosition); distToBegin = VRageMath.Vector3D.Distance(currentPosition, endPosition); } displacement = VRageMath.Vector3D.Distance(currentPosition, lastPosition); } /* r d 0 0 0 forwardThruster 0 1 1 backThruster 1 0 1 backThruster 1 1 0 forwardThruster */ public void setThrusterPower(bool direction, float power) { if (direction ^ runningDirection) { backThruster.SetValueFloat("Override", power); } else { forwardThruster.SetValueFloat("Override", power); } } public void setThrusterEnable(bool direction, bool enable) { if (enable) { if (direction ^ runningDirection) { backThruster.GetActionWithName("OnOff_On").Apply(backThruster); } else { forwardThruster.GetActionWithName("OnOff_On").Apply(forwardThruster); } } else { if (direction ^ runningDirection) { backThruster.GetActionWithName("OnOff_Off").Apply(backThruster); } else { forwardThruster.GetActionWithName("OnOff_Off").Apply(forwardThruster); } } } public void RunCU() { Echo("Currently in state " + currentState.ToString()); switch (currentState) { case 0: RunS0(); break; case 1: RunS1(); break; case 2: RunS2(); break; case 3: RunS3(); break; default: currentState = 0; break; } } public void RunS0() { double vectorDotProduct = VRageMath.Vector3D.Dot( ((runningDirection == DIR_FORWARD)?(endPosition - beginPosition):(beginPosition - endPosition)), ((runningDirection == DIR_FORWARD)?(endPosition - currentPosition):(beginPosition - currentPosition))); if ((distToEnd > ALLOW_ALIGNMENT_ERROR)&&(vectorDotProduct > 0)) { setThrusterEnable(DIR_BACK, false); if (curVel < (ALIGNMENT_VELOCITY - ALLOW_VELOCITY_ERROR)) { setThrusterPower(DIR_FORWARD, ALIGNMENT_POWER); } else if (curVel > (ALIGNMENT_VELOCITY)) { setThrusterPower(DIR_FORWARD, 0.0f); if (curVel > (ALIGNMENT_VELOCITY * 1.5)) { setThrusterEnable(DIR_BACK, true); } } } else { setThrusterPower(DIR_BACK, 0.0f); setThrusterPower(DIR_FORWARD, 0.0f); setThrusterEnable(DIR_BACK, true); setThrusterEnable(DIR_FORWARD, true); tripTime = (curTime - startTime).TotalSeconds; currentState = 1; //Wait state runningDirection = !runningDirection; //Back to begin timeToWait = DOCK_TIME; } } public void RunS1() { timeToWait -= timeDelta; if (timeToWait < 0.5) { //Running interval is 1s currentState = 2; startTime = curTime; } } public void RunS2() { if (distToEnd > (DEACCELERATE_DISTANCE + MAXIMUM_VELOCITY)) { setThrusterEnable(DIR_BACK, false); if (curVel < (MAXIMUM_VELOCITY - ALLOW_VELOCITY_ERROR)) { setThrusterPower(DIR_FORWARD, MAXIMUM_POWER); } else if (curVel > (MAXIMUM_VELOCITY)) { setThrusterPower(DIR_FORWARD, 0.0f); if (curVel > (MAXIMUM_VELOCITY * 1.2)) { setThrusterEnable(DIR_BACK, true); } } } else { currentState = 3; } } public void RunS3() { setThrusterPower(DIR_FORWARD, 0.0f); setThrusterEnable(DIR_BACK, true); if (curVel < ALIGNMENT_VELOCITY + 2*ALLOW_VELOCITY_ERROR) { setThrusterEnable(DIR_BACK, false); currentState = 0; } } //Constructor public Program() { currentState = 0; runningDirection = DIR_FORWARD; LoadStorage(); firstRun = true; lastTime = System.DateTime.Now; } //Main Method public void Main(string argument) { //Get handles lcdBlock = GridTerminalSystem.GetBlockWithName(LCD_BLOCK_NAME) as IMyTextPanel; backThruster = GridTerminalSystem.GetBlockWithName(BACK_THRUSTER_NAME) as IMyThrust; forwardThruster = GridTerminalSystem.GetBlockWithName(FORWARD_THRUSTER_NAME) as IMyThrust; if ((backThruster == null)||(forwardThruster == null)) { Echo("Unable of get Handle of Thruster!"); return; } //Get status GetCurrentPosition(); curTime = System.DateTime.Now; //Calculate calcDistances(); timeDelta = (curTime - lastTime).TotalSeconds; curVel = (displacement / timeDelta); //Save last status lastPosition = currentPosition; lastTime = curTime; if (firstRun) { firstRun = false; return; } switch (argument) { case "SetBegin": beginPosition = currentPosition; SaveStorage(); Echo("Setting B/E Position to " + Storage); break; case "SetEnd": endPosition = currentPosition; SaveStorage(); Echo("Setting B/E Position to " + Storage); break; default: Echo("Current Velocity: " + Math.Round(curVel, 4).ToString()); Echo("Distance to begin = " + Math.Round(distToBegin, 4).ToString()); Echo("Distance to end = " + Math.Round(distToEnd, 4).ToString()); if (currentState == 2 || currentState == 3) { lcdBlock.WritePublicText("当前速度 " + Math.Round(curVel, 0).ToString() + " 米/秒 \n" + "距离到站还有 " + Math.Round((tripTime - (curTime - startTime).TotalSeconds), 0).ToString() + "秒"); } else if (currentState == 0) { lcdBlock.WritePublicText("即将到站"); } else if (currentState == 1) { lcdBlock.WritePublicText("到达\n将于" + Math.Round(timeToWait, 0).ToString() + " 秒内出发"); } else { lcdBlock.WritePublicText("列车故障"); } RunCU(); break; } }