setrain.cs

//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;     
    }     
}