Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- //make a timer that triggers itself and this script
- //create 3 nav lights next to each other and name and place them: NAVC,NAVR,NAVT,NAVB
- //center,right,top must be 1 block separted, back must be in line behind center (importantish to have it exactly in line)
- //create 1 nav light named NAVAUTOPILOT, with this light the autopilot can be controlled (put it in your hotbar)
- //create 1 LCD Panel named GPSLCD , This will display all the GPS information on screen.
- //optional rename thrusters for forward thrust to: Thruster_FWD , the autopilot will turn these on/off
- //dampeners and gyros activation threshold in km from 0,0,0 when autopilot not activated
- //the autopilot gets deactivated if distance within targetmin
- const double GLB_dTargetMax = 50;
- const double GLB_dTargetMin = 0.5;
- const double GLB_kAutoPilot = 0.05; //k-factor for autopilot (if the autopilot is to agressive, decrease this value
- //default = 0.05
- //target position for navigation assist / autopilot
- const int GLB_tX = -1; //x in km
- const int GLB_tY = 0; //y in km
- const int GLB_tZ = 0; //z in km
- bool GLB_InitDone = false; //remember if initialisation has been carried out
- bool GLB_InitAutoThrust = false; //if a thruster named Thruster_FWD is detected, the autopilot will use these thrusters
- bool GLB_AutoPilotStop = false; //remember if autopilot has been activated
- int GLB_runcounter; //used to only run script every x starts
- double GLB_YawPrevious,GLB_PitchPrevious; //used for autopilot controller
- //LCD that will display position
- //cockpit that will activate dampeners
- IMyTerminalBlock GLB_LCD,GLB_Cockpit;
- //list of all gyro blocks
- List <IMyTerminalBlock> GLB_Gyros = new List <IMyTerminalBlock>();
- //list of all Thrusters forward (thrusters named Thruster_FWD
- List <IMyTerminalBlock> GLB_Thrusters = new List <IMyTerminalBlock>();
- //NAV lights
- IMyTerminalBlock GLB_NAVC,GLB_NAVR,GLB_NAVT,GLB_NAVB,GLB_NAVAUTOPILOT;
- //current X,Y,Z position with angles from target and distance to target
- double GLB_X, GLB_Y, GLB_Z, GLB_aY, GLB_aP, GLB_dTarget;
- void Main()
- {
- if (GLB_runcounter++ %10 == 0) //add 1 and check if multiple of 10
- {
- GLB_runcounter = 1;
- //initialise
- if (!GLB_InitDone)
- {
- Init();
- }
- else
- {
- UpdateTargetData();
- UpdateLcdText();
- //Activate dampeners and gyros
- if(GLB_dTarget > GLB_dTargetMax || GLB_dTarget < GLB_dTargetMin)
- {AutoDampenersGyros();}
- //Activate autopilot
- if((GLB_NAVAUTOPILOT as IMyFunctionalBlock).Enabled==true)
- {
- AutoPilot();
- GLB_AutoPilotStop = true;
- }
- else if(GLB_AutoPilotStop) //disable gyro override, stays on otherwise
- {
- AutoDampenersGyros();
- GLB_AutoPilotStop = false;
- }
- }
- }
- }
- void Init()
- {
- //Create datatype list of IMyTerminalBlock
- List<IMyTerminalBlock> blocks = new List<IMyTerminalBlock>();
- GLB_InitDone = true;
- //search for first LCD block
- //search for first cockpit block
- GridTerminalSystem.GetBlocksOfType<IMyCockpit>(blocks);
- if (blocks.Count > 0)
- {
- GLB_Cockpit = blocks[0];
- }
- else
- {GLB_InitDone = false;}
- blocks.Clear();
- //search for gyro blocks
- GridTerminalSystem.GetBlocksOfType<IMyGyro>(blocks);
- if (blocks.Count > 0)
- {
- for (int i=0; i < blocks.Count;i++)
- {
- GLB_Gyros.Add(blocks[i]);
- }
- }
- else
- {GLB_InitDone = false;}
- blocks.Clear();
- //search for gyro blocks
- GridTerminalSystem.GetBlocksOfType<IMyThrust>(blocks);
- if (blocks.Count > 0)
- {
- for (int i=0; i < blocks.Count;i++)
- {
- if (blocks[i].CustomName.Contains("Thruster_FWD"))
- {
- GLB_Thrusters.Add(blocks[i]);
- }
- }
- }
- blocks.Clear();
- //search for NAV ligths
- GridTerminalSystem.GetBlocksOfType<IMyLightingBlock>(blocks);
- for (int i = 0; i < blocks.Count; i++)
- {
- //Find center light
- if (blocks[i].CustomName.Contains("NAVC"))
- { GLB_NAVC = blocks[i]; }
- //Find top light
- if (blocks[i].CustomName.Contains("NAVT"))
- { GLB_NAVT = blocks[i]; }
- //Find back light
- if (blocks[i].CustomName.Contains("NAVB"))
- { GLB_NAVB = blocks[i]; }
- //Find right light
- if (blocks[i].CustomName.Contains("NAVR"))
- { GLB_NAVR = blocks[i]; }
- //Find autopilot light
- if (blocks[i].CustomName.Contains("NAVAUTOPILOT"))
- { GLB_NAVAUTOPILOT = blocks[i]; }
- }
- if (GLB_NAVC == null || GLB_NAVT == null || GLB_NAVB == null || GLB_NAVR == null
- || GLB_NAVAUTOPILOT == null)
- {GLB_InitDone = false;}
- blocks.Clear();
- }
- void UpdateTargetData()
- {
- 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
- //get positional data center navigation
- c_x = GLB_NAVC.GetPosition().GetDim(0)/1000;
- c_y = GLB_NAVC.GetPosition().GetDim(1)/1000;
- c_z = GLB_NAVC.GetPosition().GetDim(2)/1000;
- GLB_X = Math.Round(c_x,3);
- GLB_Y = Math.Round(c_y,3);
- GLB_Z = Math.Round(c_z,3);
- //get positional data top navigation
- t_x = GLB_NAVT.GetPosition().GetDim(0)/1000;
- t_y = GLB_NAVT.GetPosition().GetDim(1)/1000;
- t_z = GLB_NAVT.GetPosition().GetDim(2)/1000;
- //get positional data right navigation
- r_x = GLB_NAVR.GetPosition().GetDim(0)/1000;
- r_y = GLB_NAVR.GetPosition().GetDim(1)/1000;
- r_z = GLB_NAVR.GetPosition().GetDim(2)/1000;
- //get positional data back navigation
- b_x = GLB_NAVB.GetPosition().GetDim(0)/1000;
- b_y = GLB_NAVB.GetPosition().GetDim(1)/1000;
- b_z = GLB_NAVB.GetPosition().GetDim(2)/1000;
- //calculate NAVC distance to target
- //distance =sqrt(
- // sqrt((current x - target x)^2 + (current y - target y)^2)^2 //the outside sqrt and ^2 can be removed
- //+ (current z - target z)^2)
- double dNAVC = Math.Sqrt(
- Math.Pow(c_x - GLB_tX,2) + Math.Pow(c_y - GLB_tY,2) +
- Math.Pow(c_z - GLB_tZ,2));
- GLB_dTarget = Math.Round(dNAVC,3);
- //calculate NAVT distance to target (if equals NAVC then angle = 0, if +1 then 90 degrees, if -1 then -180 degrees)
- double dNAVT = Math.Sqrt(
- Math.Pow(t_x - GLB_tX,2) + Math.Pow(t_y - GLB_tY,2) +
- Math.Pow(t_z - GLB_tZ,2));
- //calculate NAVB distance to target
- double dNAVB = Math.Sqrt(
- Math.Pow(b_x - GLB_tX,2) + Math.Pow(b_y - GLB_tY,2) +
- Math.Pow(b_z - GLB_tZ,2));
- //calculate NAVR distance to target
- double dNAVR = Math.Sqrt(
- Math.Pow(r_x - GLB_tX,2) + Math.Pow(r_y - GLB_tY,2) +
- Math.Pow(r_z - GLB_tZ,2));
- //calculate distance between NAV center and NAVR back blocks
- double dNAVCR = Math.Sqrt(
- Math.Pow(c_x - r_x,2) + Math.Pow(c_y - r_y , 2) +
- Math.Pow(c_z - r_z,2));
- //calculate distance between NAV center and NAVB back blocks
- double dNAVCB = Math.Sqrt(
- Math.Pow(c_x - b_x,2) + Math.Pow(c_y - b_y , 2) +
- Math.Pow(c_z - b_z,2));
- //yaw
- //we are using 3 distances in 3d space to calculate the yaw:
- //target - NAVcenter
- //target - NAVright
- //distance between NAVcenter and NAVright
- //these wil form a triangle and we can use the angle from center to right as a source for the yaw angle
- //using the cos - rule with these points:
- //alpha = target, beta = NAVR, gamma = NAVC
- //a=dNAVCR, b=dNAVC, c=dNAVR
- //beta = invcos((a^2+c^2 - (b^2))/2*a*c)
- double CRbeta = Math.Acos((Math.Pow(dNAVCR,2) + Math.Pow(dNAVR,2) - Math.Pow(dNAVC,2)) /
- (2 * dNAVCR * dNAVR));
- //to degrees from radians
- CRbeta = (CRbeta*180) / Math.PI;
- //calculate pitch in the same way as yaw
- //beta = invcos((a^2+c^2 - (b^2))/2*a*c)
- double CTbeta = Math.Acos((Math.Pow(dNAVCR,2) + Math.Pow(dNAVT,2) - Math.Pow(dNAVC,2)) /
- (2 * dNAVCR * dNAVT));
- //to degrees from radians
- CTbeta = (CTbeta*180) / Math.PI;
- //correct to show the angle we want to see and if we are facing the other direction
- if (dNAVC < dNAVB)
- {
- CRbeta = -1 * (CRbeta - 90) ;
- CTbeta = -1 * (CTbeta - 90);
- }
- else
- {
- CRbeta = CRbeta + 90;
- CTbeta = CTbeta + 90;
- }
- if (CRbeta > 180)
- {
- CRbeta = CRbeta - 360 ;
- CTbeta = CTbeta - 360;
- }
- GLB_aY = Math.Round(CRbeta,2);
- GLB_aP = Math.Round(CTbeta,2);
- }
- void UpdateLcdText()
- {
- //set LCD Text
- IMyTextPanel GLB_LCD = (IMyTextPanel)GridTerminalSystem.GetBlockWithName("GPSLCD");
- GLB_LCD.WritePublicText(" X: " + GLB_X.ToString()
- + "\n"
- + " Y: " + GLB_Y.ToString()
- + "\n"
- + " Z: " + GLB_Z.ToString()
- + "\n"
- + " Pitch: " + GLB_aP
- + "\n"
- + " Yaw: " + GLB_aY
- + "\n"
- + " Dist.: " + GLB_dTarget);
- GLB_LCD.ShowTextureOnScreen();
- GLB_LCD.ShowPublicTextOnScreen();
- }
- void AutoDampenersGyros()
- {
- //disable override all thruster with a name that contains Thruster_FWD -found in init()-
- for (int i=0; i < GLB_Thrusters.Count; i++)
- {
- var IThruster = GLB_Thrusters[i] as IMyThrust;
- //activate override
- while (IThruster.ThrustOverride > 0)
- {
- IThruster.GetActionWithName("DecreaseOverride").Apply(IThruster);
- }
- }
- //activate dampeners
- var ICockpit = GLB_Cockpit as IMyCockpit;
- if (!ICockpit.DampenersOverride)
- {
- ICockpit.GetActionWithName("DampenersOverride").Apply(ICockpit);
- }
- //activate gyroscope
- for(int i=0; i < GLB_Gyros.Count; i++)
- {
- var IGyro = GLB_Gyros[i] as IMyGyro;
- if (IGyro.GyroOverride)
- {
- IGyro.GetActionWithName("Override").Apply(IGyro);
- }
- }
- //deactivate autopilot
- var ILightingBlock = GLB_NAVAUTOPILOT as IMyLightingBlock;
- if((GLB_NAVAUTOPILOT as IMyFunctionalBlock).Enabled)
- {
- ILightingBlock.GetActionWithName("OnOff_Off").Apply(ILightingBlock);
- }
- }
- void AutoPilot()
- {
- //calculate yaw and pitch settings for first gyro, later in the function the rest of the gyros get the same rpm's
- var IGyro = GLB_Gyros[0] as IMyGyro;
- double YawPerCyclus, DesiredYaw, PitchPerCyclus, DesiredPitch, GyroYaw,GyroPitch;
- //activate override
- if (!IGyro.GyroOverride)
- {
- IGyro.GetActionWithName("Override").Apply(IGyro);
- }
- //yaw factor
- //if yaw within 0.5 degrees, set gyro yaw rpm to zero
- if(Math.Abs(GLB_aY) < 0.5)
- {
- if(IGyro.Yaw < 0)
- {
- IGyro.GetActionWithName("IncreaseYaw").Apply(IGyro);
- }
- if(IGyro.Yaw > 0)
- {
- IGyro.GetActionWithName("DecreaseYaw").Apply(IGyro);
- }
- DesiredYaw = 0;
- YawPerCyclus = 0;
- }
- else
- {
- //calculate current yaw vector
- YawPerCyclus = (GLB_YawPrevious - GLB_aY);
- GLB_YawPrevious = GLB_aY;
- //calculate desired giro yaw rpm
- DesiredYaw = GLB_aY*GLB_kAutoPilot;
- if (YawPerCyclus > DesiredYaw && (Math.Abs((YawPerCyclus-DesiredYaw)*2) > Math.Abs(DesiredYaw)))
- {
- IGyro.GetActionWithName("IncreaseYaw").Apply(IGyro);
- }
- else if (Math.Abs((YawPerCyclus-DesiredYaw)*2) > Math.Abs(DesiredYaw))
- {
- IGyro.GetActionWithName("DecreaseYaw").Apply(IGyro);
- }
- }
- //used for slave gyros
- GyroYaw = IGyro.Yaw;
- //pitch factor
- //if pitch within 0.5 degrees, set gyro yaw rpm to zero
- if(Math.Abs(GLB_aP) < 0.5)
- {
- if(IGyro.Pitch < 0)
- {
- IGyro.GetActionWithName("IncreasePitch").Apply(IGyro);
- }
- if(IGyro.Yaw > 0)
- {
- IGyro.GetActionWithName("DecreasePitch").Apply(IGyro);
- }
- DesiredPitch = 0;
- PitchPerCyclus = 0;
- }
- else
- {
- //calculate current pitch vector
- PitchPerCyclus = (GLB_PitchPrevious - GLB_aP);
- GLB_PitchPrevious = GLB_aP;
- //calculate desired pitch vector
- DesiredPitch = GLB_aP*GLB_kAutoPilot;
- if (PitchPerCyclus < DesiredPitch && (Math.Abs((PitchPerCyclus-DesiredPitch)*2) > Math.Abs(DesiredPitch)))
- {
- IGyro.GetActionWithName("IncreasePitch").Apply(IGyro);
- }
- else if (Math.Abs((PitchPerCyclus-DesiredPitch)*2) > Math.Abs(DesiredPitch))
- {
- IGyro.GetActionWithName("DecreasePitch").Apply(IGyro);
- }
- }
- //used for slave gyros
- GyroPitch = IGyro.Pitch;
- IGyro.SetCustomName("Gyro0 des/act/rpm yaw:" +
- Math.Round(DesiredYaw,2) + "/" + Math.Round(YawPerCyclus,2) +
- "/" + Math.Round(GyroYaw,3));
- //set rest of gyro's
- for (int i=1; i < GLB_Gyros.Count; i++)
- {
- var IGyroNext = GLB_Gyros[i] as IMyGyro;
- //activate override
- if (!IGyroNext.GyroOverride)
- {
- IGyroNext.GetActionWithName("Override").Apply(IGyroNext);
- }
- if(IGyroNext.Yaw < GyroYaw)
- {
- IGyroNext.GetActionWithName("IncreaseYaw").Apply(IGyroNext);
- }
- else if(IGyroNext.Yaw > GyroYaw)
- {
- IGyroNext.GetActionWithName("DecreaseYaw").Apply(IGyroNext);
- }
- if(IGyroNext.Pitch < GyroPitch)
- {
- IGyroNext.GetActionWithName("IncreasePitch").Apply(IGyroNext);
- }
- else if(IGyroNext.Pitch > GyroPitch)
- {
- IGyroNext.GetActionWithName("DecreasePitch").Apply(IGyroNext);
- }
- IGyroNext.SetCustomName("Gyro" + i + " des/act/rpm pitch:"
- + Math.Round(DesiredPitch,2) + "/" + Math.Round(PitchPerCyclus,2) + "/" + Math.Round(GyroPitch,3));
- }
- //override all thruster with a name that contains Thruster_FWD -found in init()- to max
- for (int i=0; i < GLB_Thrusters.Count; i++)
- {
- var IThruster = GLB_Thrusters[i] as IMyThrust;
- //activate override
- if (IThruster.ThrustOverride < 1100000) //max thrust = 1.200.000
- {
- IThruster.GetActionWithName("IncreaseOverride").Apply(IThruster);
- }
- }
- }
- /*
- //didn't know you could use block.GetPosition().GetDim(0)....
- //used GetPosition() instead and split the string
- //function can be removed but keep it for now, nice example
- void UpdatePos()
- {
- string position = GLB_NAVC.GetPosition().ToString();
- //string position = (GLB_Cockpit as IMyCubeBlock).GetPosition().ToString(); //makes no difference
- //string position = (GLB_Cockpit as IMyTerminalBlock).GetPosition().ToString(); //makes no difference
- //positions in ship grid (broken, x/y/z are the same, also broken in world grid)
- //GLB_Antenna.SetCustomName(position.X.ToString() + " " + position.Y.ToString() + " " + position.Z.ToString());
- //split string in x, y and z
- //string[] coords = position.Split(new Char [] {':', ' ', 'X', 'Y', 'Z'}); //doesn't work?
- //manual split
- string CurCoord = "";
- int state = 0;
- for (int i = 0; i < position.Length; i++)
- {
- switch (state)
- {
- case 0:
- if (position[i]=='X')
- {
- CurCoord = "";
- i++; //skip over ":"
- state = 10;
- }
- break;
- //X
- case 10: //switch case is partial broken, if this case would be 2 then exception
- if (position[i]=='Y')
- {
- //store X coord
- GLB_X = Math.Round(Convert.ToDouble(CurCoord)/1000,3);
- CurCoord = "";
- i++; //skip over ":"
- state = 20;
- break;
- }
- CurCoord += position[i];
- break;
- //Y
- case 20:
- if (position[i]=='Z')
- {
- //store Y coord
- GLB_Y = Math.Round(Convert.ToDouble(CurCoord)/1000,3);
- CurCoord = "";
- i++; //skip over ":"
- state = 30;
- break;
- }
- CurCoord += position[i];
- break;
- //Z
- case 30:
- if (position[i]=='}')
- {
- //store Z coord
- GLB_Z = Math.Round(Convert.ToDouble(CurCoord)/1000,3);
- CurCoord = "";
- state = 40;
- break;
- }
- CurCoord += position[i];
- break;
- default:
- break;
- }
- }
- }
- */
Advertisement
Add Comment
Please, Sign In to add comment