Morphik08

Space Engineers AutoPilot GPS AutoStop LCD Edit

Mar 13th, 2015
580
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C# 18.42 KB | None | 0 0
  1.         //make a timer that triggers itself and this script
  2. //create 3 nav lights next to each other and name and place them: NAVC,NAVR,NAVT,NAVB
  3. //center,right,top must be 1 block separted, back must be in line behind center (importantish to have it exactly in line)
  4. //create 1 nav light named NAVAUTOPILOT, with this light the autopilot can be controlled (put it in your hotbar)
  5. //create 1 LCD Panel named GPSLCD , This will display all the GPS information on screen.
  6. //optional rename thrusters for forward thrust to: Thruster_FWD , the autopilot will turn these on/off
  7.  
  8.  
  9. //dampeners and gyros activation threshold in km from 0,0,0 when autopilot not activated  
  10. //the autopilot gets deactivated if distance within targetmin
  11. const double GLB_dTargetMax  = 50;
  12. const double GLB_dTargetMin = 0.5;
  13. const double GLB_kAutoPilot = 0.05; //k-factor for autopilot (if the autopilot is to agressive, decrease this value  
  14.                                                             //default = 0.05
  15.  
  16. //target position for navigation assist / autopilot
  17. const int GLB_tX = -1; //x in km
  18. const int GLB_tY = 0; //y in km
  19. const int GLB_tZ = 0; //z in km
  20.  
  21. bool GLB_InitDone = false; //remember if initialisation has been carried out
  22. bool GLB_InitAutoThrust = false; //if a thruster named Thruster_FWD is detected, the autopilot will use these thrusters
  23. bool GLB_AutoPilotStop = false; //remember if autopilot has been activated
  24. int GLB_runcounter; //used to only run script every x starts  
  25. double GLB_YawPrevious,GLB_PitchPrevious; //used for autopilot controller
  26.  
  27. //LCD that will display position
  28. //cockpit that will activate dampeners
  29. IMyTerminalBlock GLB_LCD,GLB_Cockpit;
  30.  
  31. //list of all gyro blocks
  32. List <IMyTerminalBlock> GLB_Gyros = new List <IMyTerminalBlock>();
  33.  
  34. //list of all Thrusters forward (thrusters named Thruster_FWD
  35. List <IMyTerminalBlock> GLB_Thrusters = new List <IMyTerminalBlock>();
  36.  
  37. //NAV lights
  38. IMyTerminalBlock  GLB_NAVC,GLB_NAVR,GLB_NAVT,GLB_NAVB,GLB_NAVAUTOPILOT;
  39.  
  40. //current X,Y,Z position with angles from target and distance to target
  41. double GLB_X, GLB_Y, GLB_Z, GLB_aY, GLB_aP, GLB_dTarget;
  42.  
  43. void Main()
  44. {
  45.  if (GLB_runcounter++ %10 == 0) //add 1 and check if multiple of 10
  46.   {
  47.     GLB_runcounter = 1;
  48.     //initialise  
  49.     if (!GLB_InitDone)
  50.     {
  51.         Init();
  52.     }
  53.     else
  54.     {
  55.         UpdateTargetData();
  56.         UpdateLcdText();
  57.  
  58.          //Activate dampeners and gyros
  59.          if(GLB_dTarget > GLB_dTargetMax || GLB_dTarget < GLB_dTargetMin)  
  60.          {AutoDampenersGyros();}
  61.      
  62.          //Activate autopilot
  63.         if((GLB_NAVAUTOPILOT as IMyFunctionalBlock).Enabled==true)
  64.          {
  65.             AutoPilot();
  66.             GLB_AutoPilotStop = true;
  67.          }
  68.          else if(GLB_AutoPilotStop)  //disable gyro override, stays on otherwise
  69.          {
  70.             AutoDampenersGyros();
  71.             GLB_AutoPilotStop = false;
  72.          }  
  73.      }
  74.   }
  75. }
  76.  
  77. void Init()
  78. {
  79.     //Create datatype list of IMyTerminalBlock
  80.     List<IMyTerminalBlock> blocks = new List<IMyTerminalBlock>();  
  81.     GLB_InitDone  = true;
  82.  
  83.     //search for first LCD block  
  84.    
  85.  
  86.     //search for first cockpit block    
  87.     GridTerminalSystem.GetBlocksOfType<IMyCockpit>(blocks);    
  88.     if (blocks.Count > 0)  
  89.     {  
  90.         GLB_Cockpit = blocks[0];  
  91.     }
  92.     else  
  93.     {GLB_InitDone = false;}
  94.     blocks.Clear();
  95.  
  96.     //search for gyro blocks
  97.     GridTerminalSystem.GetBlocksOfType<IMyGyro>(blocks);    
  98.     if (blocks.Count > 0)    
  99.     {    
  100.         for (int i=0; i < blocks.Count;i++)  
  101.         {
  102.             GLB_Gyros.Add(blocks[i]);
  103.         }
  104.     }  
  105.     else  
  106.     {GLB_InitDone = false;}
  107.     blocks.Clear();
  108.  
  109.     //search for gyro blocks  
  110.     GridTerminalSystem.GetBlocksOfType<IMyThrust>(blocks);      
  111.     if (blocks.Count > 0)    
  112.     {    
  113.         for (int i=0; i < blocks.Count;i++)  
  114.         {  
  115.             if (blocks[i].CustomName.Contains("Thruster_FWD"))
  116.             {
  117.                 GLB_Thrusters.Add(blocks[i]);                
  118.             }
  119.         }  
  120.     }  
  121.     blocks.Clear();
  122.  
  123.     //search for NAV ligths
  124.     GridTerminalSystem.GetBlocksOfType<IMyLightingBlock>(blocks);    
  125.     for (int i = 0; i < blocks.Count; i++)    
  126.     {    
  127.         //Find center light
  128.         if (blocks[i].CustomName.Contains("NAVC"))    
  129.         { GLB_NAVC = blocks[i]; }
  130.  
  131.         //Find top light  
  132.         if (blocks[i].CustomName.Contains("NAVT"))    
  133.         { GLB_NAVT = blocks[i]; }
  134.  
  135.         //Find back light    
  136.         if (blocks[i].CustomName.Contains("NAVB"))      
  137.         { GLB_NAVB = blocks[i]; }
  138.  
  139.         //Find right light  
  140.         if (blocks[i].CustomName.Contains("NAVR"))
  141.         { GLB_NAVR = blocks[i]; }
  142.          
  143.         //Find autopilot light
  144.         if (blocks[i].CustomName.Contains("NAVAUTOPILOT"))
  145.         { GLB_NAVAUTOPILOT = blocks[i]; }
  146.     }
  147.     if (GLB_NAVC == null ||  GLB_NAVT == null || GLB_NAVB == null || GLB_NAVR == null  
  148.         || GLB_NAVAUTOPILOT == null)
  149.     {GLB_InitDone = false;}
  150.     blocks.Clear();
  151. }
  152.  
  153.  
  154. void UpdateTargetData()
  155. {  
  156.     double c_x,c_y,c_z,t_x,t_y,t_z,r_x,r_y,r_z,b_x,b_y,b_z; //Nav lights positions, center top right
  157.  
  158.     //get positional data center navigation
  159.     c_x = GLB_NAVC.GetPosition().GetDim(0)/1000;
  160.     c_y = GLB_NAVC.GetPosition().GetDim(1)/1000;
  161.     c_z = GLB_NAVC.GetPosition().GetDim(2)/1000;
  162.     GLB_X = Math.Round(c_x,3);
  163.     GLB_Y = Math.Round(c_y,3);
  164.     GLB_Z = Math.Round(c_z,3);
  165.      
  166.    //get positional data top navigation  
  167.     t_x = GLB_NAVT.GetPosition().GetDim(0)/1000;  
  168.     t_y = GLB_NAVT.GetPosition().GetDim(1)/1000;  
  169.     t_z = GLB_NAVT.GetPosition().GetDim(2)/1000;
  170.    
  171.     //get positional data right navigation  
  172.     r_x = GLB_NAVR.GetPosition().GetDim(0)/1000;  
  173.     r_y = GLB_NAVR.GetPosition().GetDim(1)/1000;  
  174.     r_z = GLB_NAVR.GetPosition().GetDim(2)/1000;
  175.  
  176.     //get positional data back navigation  
  177.     b_x = GLB_NAVB.GetPosition().GetDim(0)/1000;  
  178.     b_y = GLB_NAVB.GetPosition().GetDim(1)/1000;  
  179.     b_z = GLB_NAVB.GetPosition().GetDim(2)/1000;
  180.  
  181.     //calculate NAVC distance to target
  182.     //distance =sqrt(
  183.                         // sqrt((current x - target x)^2 + (current y - target y)^2)^2     //the outside sqrt and ^2 can be removed
  184.                         //+ (current z - target z)^2)
  185.     double dNAVC = Math.Sqrt(  
  186.                                 Math.Pow(c_x - GLB_tX,2) + Math.Pow(c_y - GLB_tY,2) +  
  187.                                  Math.Pow(c_z - GLB_tZ,2));
  188.     GLB_dTarget = Math.Round(dNAVC,3);
  189.      
  190.     //calculate NAVT distance to target (if equals NAVC then angle = 0, if +1 then 90 degrees, if -1 then -180 degrees)
  191.     double dNAVT =  Math.Sqrt(  
  192.                                 Math.Pow(t_x - GLB_tX,2) + Math.Pow(t_y - GLB_tY,2) +    
  193.                                  Math.Pow(t_z - GLB_tZ,2));
  194.      
  195.    //calculate NAVB distance to target  
  196.     double dNAVB =  Math.Sqrt(    
  197.                                 Math.Pow(b_x - GLB_tX,2) + Math.Pow(b_y - GLB_tY,2) +    
  198.                                  Math.Pow(b_z - GLB_tZ,2));
  199.      
  200.    //calculate NAVR distance to target
  201.     double dNAVR =  Math.Sqrt(    
  202.                                 Math.Pow(r_x - GLB_tX,2) + Math.Pow(r_y - GLB_tY,2) +    
  203.                                  Math.Pow(r_z - GLB_tZ,2));  
  204.      
  205.     //calculate distance between NAV center and NAVR back blocks  
  206.     double dNAVCR = Math.Sqrt(      
  207.                                 Math.Pow(c_x - r_x,2) + Math.Pow(c_y - r_y , 2) +      
  208.                                  Math.Pow(c_z - r_z,2));
  209.  
  210.     //calculate distance between NAV center and NAVB back blocks
  211.     double dNAVCB = Math.Sqrt(    
  212.                                 Math.Pow(c_x - b_x,2) + Math.Pow(c_y - b_y , 2) +      
  213.                                  Math.Pow(c_z - b_z,2));
  214.  
  215.  
  216.     //yaw
  217.     //we are using 3 distances in 3d space to calculate the yaw:
  218.     //target - NAVcenter
  219.     //target - NAVright
  220.     //distance between NAVcenter and NAVright
  221.     //these wil form a triangle and we can use the angle from center to right as a source for the yaw angle
  222.  
  223.     //using the cos - rule with these points:
  224.     //alpha = target, beta = NAVR, gamma = NAVC  
  225.     //a=dNAVCR, b=dNAVC, c=dNAVR
  226.  
  227.     //beta = invcos((a^2+c^2 - (b^2))/2*a*c)
  228.     double CRbeta = Math.Acos((Math.Pow(dNAVCR,2) + Math.Pow(dNAVR,2) - Math.Pow(dNAVC,2)) /    
  229.                      (2 * dNAVCR * dNAVR));  
  230.  
  231.     //to degrees from radians
  232.     CRbeta = (CRbeta*180) / Math.PI;
  233.  
  234.  
  235.     //calculate pitch in the same way as yaw
  236.     //beta = invcos((a^2+c^2 - (b^2))/2*a*c)  
  237.     double CTbeta = Math.Acos((Math.Pow(dNAVCR,2) + Math.Pow(dNAVT,2) - Math.Pow(dNAVC,2)) /    
  238.                      (2 * dNAVCR * dNAVT));  
  239.     //to degrees from radians  
  240.     CTbeta = (CTbeta*180) / Math.PI;
  241.  
  242.     //correct to show the angle we want to see and if we are facing the other direction
  243.     if (dNAVC < dNAVB)  
  244.     {  
  245.         CRbeta = -1 * (CRbeta - 90) ;  
  246.         CTbeta = -1 * (CTbeta - 90);
  247.     }  
  248.     else  
  249.     {  
  250.         CRbeta = CRbeta + 90;
  251.         CTbeta = CTbeta + 90;  
  252.     }  
  253.     if (CRbeta > 180)  
  254.     {  
  255.         CRbeta = CRbeta - 360 ;  
  256.         CTbeta = CTbeta - 360;
  257.     }    
  258.  
  259.     GLB_aY = Math.Round(CRbeta,2);
  260.     GLB_aP = Math.Round(CTbeta,2);
  261. }
  262.  
  263. void UpdateLcdText()
  264. {
  265.    //set LCD Text  
  266.    IMyTextPanel GLB_LCD = (IMyTextPanel)GridTerminalSystem.GetBlockWithName("GPSLCD");
  267.     GLB_LCD.WritePublicText(" X: " + GLB_X.ToString()
  268.                 + "\n"
  269.                 + " Y: " + GLB_Y.ToString()  
  270.                             + "\n"
  271.                 + " Z: " + GLB_Z.ToString()
  272.                 + "\n"
  273.                 + " Pitch: " + GLB_aP
  274.                 + "\n"
  275.                 + " Yaw: " + GLB_aY
  276.                 + "\n"
  277.                 + " Dist.: " + GLB_dTarget);
  278.     GLB_LCD.ShowTextureOnScreen();
  279.     GLB_LCD.ShowPublicTextOnScreen();  
  280. }
  281.  
  282.  
  283. void AutoDampenersGyros()
  284. {    
  285.    
  286. //disable override all thruster with a name that contains Thruster_FWD -found in init()-  
  287.     for (int i=0; i < GLB_Thrusters.Count; i++)  
  288.     {  
  289.         var IThruster = GLB_Thrusters[i] as IMyThrust;  
  290.  
  291.         //activate override  
  292.         while (IThruster.ThrustOverride > 0)      
  293.         {      
  294.             IThruster.GetActionWithName("DecreaseOverride").Apply(IThruster);      
  295.         }  
  296.     }    
  297. //activate dampeners
  298.     var ICockpit = GLB_Cockpit as IMyCockpit;
  299.     if (!ICockpit.DampenersOverride)
  300.         {
  301.          ICockpit.GetActionWithName("DampenersOverride").Apply(ICockpit);
  302.         }
  303.  
  304.     //activate gyroscope
  305.     for(int i=0; i < GLB_Gyros.Count; i++)
  306.     {
  307.         var IGyro = GLB_Gyros[i] as IMyGyro;
  308.         if (IGyro.GyroOverride)  
  309.         {  
  310.            IGyro.GetActionWithName("Override").Apply(IGyro);  
  311.         }
  312.     }
  313.  
  314.     //deactivate autopilot
  315.     var ILightingBlock = GLB_NAVAUTOPILOT as IMyLightingBlock;
  316.     if((GLB_NAVAUTOPILOT as IMyFunctionalBlock).Enabled)  
  317.     {  
  318.         ILightingBlock.GetActionWithName("OnOff_Off").Apply(ILightingBlock);  
  319.     }  
  320.  }
  321.  
  322.  
  323. void AutoPilot()
  324. {    
  325. //calculate yaw and pitch settings for first gyro, later in the function the rest of the gyros get the same rpm's
  326.  
  327.     var IGyro = GLB_Gyros[0] as IMyGyro;
  328.     double YawPerCyclus, DesiredYaw, PitchPerCyclus, DesiredPitch, GyroYaw,GyroPitch;
  329.  
  330. //activate override
  331.     if (!IGyro.GyroOverride)  
  332.     {  
  333.         IGyro.GetActionWithName("Override").Apply(IGyro);  
  334.     }
  335.  
  336. //yaw factor
  337.  
  338.     //if yaw within 0.5 degrees, set gyro yaw rpm to zero
  339.     if(Math.Abs(GLB_aY) < 0.5)
  340.     {
  341.         if(IGyro.Yaw < 0)  
  342.         {
  343.             IGyro.GetActionWithName("IncreaseYaw").Apply(IGyro);  
  344.         }
  345.      
  346.         if(IGyro.Yaw > 0)  
  347.         {  
  348.             IGyro.GetActionWithName("DecreaseYaw").Apply(IGyro);  
  349.         }
  350.              
  351.             DesiredYaw = 0;  
  352.             YawPerCyclus = 0;
  353.     }
  354.     else
  355.     {
  356.         //calculate current yaw vector
  357.         YawPerCyclus = (GLB_YawPrevious - GLB_aY);
  358.         GLB_YawPrevious = GLB_aY;
  359.  
  360.         //calculate desired giro yaw rpm
  361.         DesiredYaw = GLB_aY*GLB_kAutoPilot;  
  362.  
  363.         if (YawPerCyclus > DesiredYaw && (Math.Abs((YawPerCyclus-DesiredYaw)*2) > Math.Abs(DesiredYaw)))  
  364.         {  
  365.             IGyro.GetActionWithName("IncreaseYaw").Apply(IGyro);    
  366.         }  
  367.         else  if (Math.Abs((YawPerCyclus-DesiredYaw)*2) > Math.Abs(DesiredYaw))
  368.         {    
  369.             IGyro.GetActionWithName("DecreaseYaw").Apply(IGyro);    
  370.         }
  371.     }
  372.  
  373.     //used for slave gyros
  374.     GyroYaw = IGyro.Yaw;
  375.  
  376. //pitch factor
  377.  
  378.     //if pitch within 0.5 degrees, set gyro yaw rpm to zero  
  379.     if(Math.Abs(GLB_aP) < 0.5)  
  380.     {  
  381.         if(IGyro.Pitch < 0)  
  382.         {  
  383.             IGyro.GetActionWithName("IncreasePitch").Apply(IGyro);  
  384.         }  
  385.      
  386.         if(IGyro.Yaw > 0)    
  387.         {  
  388.             IGyro.GetActionWithName("DecreasePitch").Apply(IGyro);    
  389.         }  
  390.                
  391.             DesiredPitch = 0;  
  392.             PitchPerCyclus = 0;  
  393.     }  
  394.     else  
  395.     {  
  396.         //calculate current pitch vector  
  397.        PitchPerCyclus = (GLB_PitchPrevious - GLB_aP);  
  398.         GLB_PitchPrevious = GLB_aP;  
  399.  
  400.         //calculate desired pitch vector  
  401.         DesiredPitch = GLB_aP*GLB_kAutoPilot;  
  402.        
  403.         if (PitchPerCyclus < DesiredPitch && (Math.Abs((PitchPerCyclus-DesiredPitch)*2) > Math.Abs(DesiredPitch)))
  404.         {  
  405.             IGyro.GetActionWithName("IncreasePitch").Apply(IGyro);  
  406.         }  
  407.         else if (Math.Abs((PitchPerCyclus-DesiredPitch)*2) > Math.Abs(DesiredPitch))  
  408.         {  
  409.             IGyro.GetActionWithName("DecreasePitch").Apply(IGyro);    
  410.         }
  411.     }  
  412.  
  413.     //used for slave gyros
  414.     GyroPitch = IGyro.Pitch;
  415.      
  416.    IGyro.SetCustomName("Gyro0 des/act/rpm yaw:" +
  417.                                           Math.Round(DesiredYaw,2) + "/" + Math.Round(YawPerCyclus,2) +  
  418.                                           "/" + Math.Round(GyroYaw,3));  
  419.    
  420. //set rest of gyro's
  421.  
  422.     for (int i=1; i < GLB_Gyros.Count; i++)
  423.     {    
  424.         var IGyroNext = GLB_Gyros[i] as IMyGyro;
  425.          
  426.         //activate override  
  427.         if (!IGyroNext.GyroOverride)    
  428.         {    
  429.             IGyroNext.GetActionWithName("Override").Apply(IGyroNext);    
  430.         }
  431.  
  432.         if(IGyroNext.Yaw < GyroYaw)
  433.         {
  434.              IGyroNext.GetActionWithName("IncreaseYaw").Apply(IGyroNext);  
  435.         }
  436.         else if(IGyroNext.Yaw > GyroYaw)
  437.         {
  438.                 IGyroNext.GetActionWithName("DecreaseYaw").Apply(IGyroNext);  
  439.         }
  440.  
  441.         if(IGyroNext.Pitch < GyroPitch)  
  442.         {  
  443.              IGyroNext.GetActionWithName("IncreasePitch").Apply(IGyroNext);    
  444.         }  
  445.         else if(IGyroNext.Pitch > GyroPitch)  
  446.         {  
  447.                 IGyroNext.GetActionWithName("DecreasePitch").Apply(IGyroNext);    
  448.         }
  449.  
  450.          IGyroNext.SetCustomName("Gyro" + i + " des/act/rpm pitch:"  
  451.         + Math.Round(DesiredPitch,2) + "/" + Math.Round(PitchPerCyclus,2) + "/" + Math.Round(GyroPitch,3));  
  452.    
  453.     }
  454.  
  455.  
  456. //override all thruster with a name that contains Thruster_FWD -found in init()- to max
  457.     for (int i=0; i < GLB_Thrusters.Count; i++)
  458.     {
  459.         var IThruster = GLB_Thrusters[i] as IMyThrust;
  460.  
  461.         //activate override
  462.         if (IThruster.ThrustOverride < 1100000) //max thrust = 1.200.000
  463.         {    
  464.             IThruster.GetActionWithName("IncreaseOverride").Apply(IThruster);    
  465.         }
  466.     }
  467. }
  468.  
  469. /*
  470. //didn't know you could use block.GetPosition().GetDim(0)....  
  471. //used GetPosition() instead and split the string  
  472. //function can be removed but keep it for now, nice example  
  473. void UpdatePos()  
  474. {  
  475.     string position = GLB_NAVC.GetPosition().ToString();  
  476.  
  477.     //string position = (GLB_Cockpit as IMyCubeBlock).GetPosition().ToString(); //makes no difference  
  478.     //string position = (GLB_Cockpit as IMyTerminalBlock).GetPosition().ToString(); //makes no difference  
  479.  
  480.     //positions in ship grid (broken, x/y/z are the same, also broken in world grid)  
  481.     //GLB_Antenna.SetCustomName(position.X.ToString() + "  " + position.Y.ToString() + "  " + position.Z.ToString());  
  482.      
  483.     //split string in x, y and z  
  484.     //string[] coords = position.Split(new Char [] {':', ' ', 'X', 'Y', 'Z'}); //doesn't work?  
  485.  
  486.     //manual split  
  487.     string CurCoord = "";  
  488.     int state = 0;  
  489.     for (int i = 0; i < position.Length; i++)  
  490.     {  
  491.             switch (state)  
  492.             {      
  493.             case 0:  
  494.                 if (position[i]=='X')  
  495.                 {    
  496.                     CurCoord = "";  
  497.                     i++; //skip over ":"  
  498.                     state = 10;  
  499.                 }  
  500.                 break;  
  501.              
  502.             //X  
  503.             case 10: //switch case is partial broken, if this case would be 2 then exception  
  504.             if (position[i]=='Y')  
  505.                 {  
  506.                     //store X coord  
  507.                     GLB_X = Math.Round(Convert.ToDouble(CurCoord)/1000,3);  
  508.                     CurCoord = "";  
  509.                     i++; //skip over ":"  
  510.                    state = 20;  
  511.                    break;  
  512.                }  
  513.                 CurCoord += position[i];  
  514.                 break;  
  515.  
  516.             //Y  
  517.                 case 20:  
  518.                     if (position[i]=='Z')    
  519.                     {  
  520.                         //store Y coord    
  521.                         GLB_Y = Math.Round(Convert.ToDouble(CurCoord)/1000,3);  
  522.                         CurCoord = "";  
  523.                         i++; //skip over ":"    
  524.                         state = 30;  
  525.                         break;  
  526.                     }  
  527.                     CurCoord += position[i];  
  528.                     break;  
  529.  
  530.             //Z  
  531.             case 30:  
  532.                     if (position[i]=='}')    
  533.                     {  
  534.                         //store Z coord    
  535.                         GLB_Z = Math.Round(Convert.ToDouble(CurCoord)/1000,3);  
  536.                         CurCoord = "";  
  537.                         state = 40;  
  538.                         break;  
  539.                     }  
  540.                     CurCoord += position[i];  
  541.                     break;  
  542.  
  543.          
  544.             default:  
  545.             break;  
  546.             }  
  547.     }  
  548.  }
  549. */
Advertisement
Add Comment
Please, Sign In to add comment