Skip to content

Instantly share code, notes, and snippets.

@zephray
Last active November 13, 2017 01:41
Show Gist options
  • Save zephray/c258426cbc4fc3ca2ba889906295c3f4 to your computer and use it in GitHub Desktop.
Save zephray/c258426cbc4fc3ca2ba889906295c3f4 to your computer and use it in GitHub Desktop.
//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;
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment