public void Init(GyroControl gc) { for (int i = 0; i < maxDepth; ++i) { insertData(gc); } }
// Start is called before the first frame update void Start() { // have timeLeft track our time until the first flip at the start of the game timeLeft = timeBetweenFlips; // find a GyroControl to attach to this script gc = FindObjectOfType <GyroControl>(); }
/** * Store a reference to the GridTerminalSystem, identify the controller and initialize * the features, if they are enabled / runnable */ public void init(IMyGridTerminalSystem gridTerminalSystem) { // Get references to the GridTerminalSystem and the (preferred) control seat registry.gridTerminalSystem = gridTerminalSystem; registry.controller = identifyController(gridTerminalSystem, config.preferredControllerName); // Get references to the ship's thrusters, grouped by direction registry.thrusters = ThrustControl.identifyThrusters(registry); // Get references to the ship's gyros registry.gyros = GyroControl.identifyGyros(registry); // Initialize the log log.init(gridTerminalSystem); // Initialize the sensors sensors.init(registry); // Initialize the controls controls.init(registry); // Log the controller name log.info("Controller: " + registry.controller.CustomName); // Initialize the features features.ForEach(wrappedFeature => { if (shouldRunFeature(wrappedFeature)) { log.info("Initializing feature [" + wrappedFeature.feature.getName() + "]"); wrappedFeature.feature.init(registry, log, controls, sensors, config); wrappedFeature.nameUCase = wrappedFeature.feature.getName().ToUpper(); } }); }
void Start() { characterController = cameraContainer.GetComponent <CharacterController>(); cameraObject = cameraContainer.transform.GetChild(0).gameObject; cam = cameraObject.GetComponent <Camera>(); gyroControl = cameraContainer.GetComponent <GyroControl>(); }
public void init(FlightAssistantRegistry registry, Log log, FlightAssistantControls controls, Sensors sensors, FlightAssistantConfig config) { this.config = config; this.sensors = sensors; this.thrustControl = controls.thrustControl; this.gyroControl = controls.gyroControl; this.registry = registry; this.log = log; }
public cl_Ship_Pirate() { tags.Add(SHIP); tags.Add(PIRATE); direction = Random.onUnitSphere; speed = Random.Range(0.75f, 1.25f); weapons.Add(new cl_W_LowDmg()); gyro = GameObject.Find("Gyro").GetComponent <GyroControl>(); hpMax = hp = 30; }
private void Start() { Instance = this; DontDestroyOnLoad(gameObject); //cameraContainer = new GameObject("Camera Container"); //cameraContainer.transform.position = transform.position; //transform.SetParent(cameraContainer.transform); gyroEnabled = EnabledGyro(); }
Program() { gridSystem = GridTerminalSystem; gridId = Me.CubeGrid.EntityId; rc = GetBlock <IMyRemoteControl>(); thrust = new ThrusterControl(); gyros = new GyroControl(); Runtime.UpdateFrequency = UpdateFrequency.Update1; Clock.Initialize(UpdateFrequency.Update1); }
public Program() { settings = new Settings(Me); settings.Load(); storage = new StorageData(this); storage.Load(); transmitTag += settings.followerSystemId.Value; transmitCommandTag += settings.followerSystemId.Value; // Prioritize the given cockpit name rc = GetBlock <IMyShipController>(settings.cockpitName.Value, true); if (rc == null) // Second priority cockpit { rc = GetBlock <IMyCockpit>(); } if (rc == null) // Third priority remote control { rc = GetBlock <IMyRemoteControl>(); } if (rc == null) // No cockpits found. { throw new Exception("No cockpit/remote control found. Set the cockpitName field in settings."); } thrust = new ThrusterControl(rc, settings.tickSpeed.Value, GetBlocks <IMyThrust>()); gyros = new GyroControl(rc, settings.tickSpeed.Value, GetBlocks <IMyGyro>()); if (settings.enableCollisionAvoidence.Value) { cameras = GetBlocks <IMyCameraBlock>(); } if (settings.tickSpeed.Value == UpdateFrequency.Update10) { echoFrequency = 10; } else if (settings.tickSpeed.Value == UpdateFrequency.Update100) { echoFrequency = 1; } leaderListener = IGC.RegisterBroadcastListener(transmitTag); leaderListener.SetMessageCallback(""); commandListener = IGC.RegisterBroadcastListener(transmitCommandTag); commandListener.SetMessageCallback(""); Echo("Ready."); if (!storage.IsDisabled) { Runtime.UpdateFrequency = settings.tickSpeed.Value; } }
// Utility public void Reset(bool gyroOverride = false, bool?thrusterEnable = true, Func <IMyThrust, bool> thrusterCondition = null) { GyroControl.Reset(); GyroControl.EnableOverride(gyroOverride); ThrustControl.Reset(thrusterCondition); if (thrusterEnable != null) { ThrustControl.Enable((bool)thrusterEnable, thrusterCondition); } }
private static void DT_Input_RemoveGyroControlFromCamera() { GameObject cameraGameObject = Selection.activeGameObject; GyroControl gyroCamScript = cameraGameObject.GetComponent <GyroControl>(); if (gyroCamScript == null) { Debug.LogWarning("Camera is was not gyro controlled."); return; } Undo.DestroyObjectImmediate(cameraGameObject.GetComponent <GyroControl>()); Debug.Log("\"" + cameraGameObject.name + "\" has its gyroscope controls removed."); }
public cl_Ship_Trader() : base() { hpMax = hp = 30; tags.Add(SHIP); tags.Add(TRADER); direction = Random.onUnitSphere; speed = Random.Range(0.75f, 1.25f); gyro = GameObject.Find("Gyro").GetComponent <GyroControl>(); for (int i = 0; i < Random.Range(2, 4); i++) { switch (Random.Range(0, 5)) { case 0: Inventory.Add(new cl_W_Default()); weaponVal = Random.Range(5, 10); invText += "Default Weapon " + weaponVal + "\n"; break; case 1: Inventory.Add(new cl_W_HighDmg()); weaponVal = Random.Range(10, 30); invText += "HighDmg Weapon " + weaponVal + "\n"; break; case 2: Inventory.Add(new cl_W_LowDmg()); weaponVal = Random.Range(2, 5); invText += "LowDmg Weapon " + weaponVal + "\n"; break; case 3: Inventory.Add(new cl_W_Explosive()); weaponVal = Random.Range(40, 50); invText += "Explosive Weapon " + weaponVal + "\n"; break; case 4: Inventory.Add(new cl_W_Energy()); weaponVal = Random.Range(60, 80); invText += "Energy Weapon " + weaponVal + "\n"; break; } } }
public void LaunchPlate() { if (platesLeft > 0 && launchPlateStatus == true) { platesLeft--; GameObject Plate = Instantiate(prefabPlate) as GameObject; Plate.transform.parent = parentGameObject.transform; Vector3 spawner = new Vector3((transform.localPosition.x - Random.Range(-6, 6)), transform.localPosition.y, transform.localPosition.z); Plate.transform.position = spawner + Camera.main.transform.forward * -5; Rigidbody rb = Plate.GetComponent <Rigidbody>(); rb.velocity = Camera.main.transform.up * 40; launchPlateStatus = false; GyroControl disableGyro = cameraGyro.GetComponent <GyroControl>(); disableGyro.disableGyroTemp = true; //timerPlate = 3.0f; //tempPlate = platesLeft; } }
//Constructors public cl_Station_Trader() : base() { tags.Add(STATION); tags.Add(TRADER); gyro = GameObject.Find("Gyro").GetComponent <GyroControl>(); for (int i = 0; i < Random.Range(2, 4); i++) { switch (Random.Range(0, 5)) { case 0: Inventory.Add(new cl_W_Default()); weaponVal = Random.Range(5, 10); invText += "Default Weapon " + weaponVal + "\n"; break; case 1: Inventory.Add(new cl_W_HighDmg()); weaponVal = Random.Range(10, 30); invText += "HighDmg Weapon " + weaponVal + "\n"; break; case 2: Inventory.Add(new cl_W_LowDmg()); weaponVal = Random.Range(2, 5); invText += "LowDmg Weapon " + weaponVal + "\n"; break; case 3: Inventory.Add(new cl_W_Explosive()); weaponVal = Random.Range(40, 50); invText += "Explosive Weapon " + weaponVal + "\n"; break; case 4: Inventory.Add(new cl_W_Energy()); weaponVal = Random.Range(60, 80); invText += "Energy Weapon " + weaponVal + "\n"; break; } } }
void Update() { // Touch (Left mouse button) /*if (GyroControl.eyeView == true) { * if (Input.GetMouseButtonDown (0)) { * mouseTimer = 0f; * mousePosition = Input.mousePosition; * } * if (Input.GetMouseButton (0)) { * mouseTimer += Time.deltaTime; * * if (mouseTimer > tapTime) { * GyroControl.RotateCamera (GetCameraRotation ()); * } * * mousePosition = Input.mousePosition; * } * if (Input.GetMouseButtonUp (0)) { * if (mouseTimer < tapTime) { * TeleportToMouseClick (); * } * } * }*/ // Right mouse button if (!GyroControl.gyroEnabled) { if (Input.GetMouseButtonDown(1)) { mousePosition = Input.mousePosition; } if (Input.GetMouseButton(1)) { mouseTimer += Time.deltaTime; GyroControl.RotateCamera(GetCameraRotation()); mousePosition = Input.mousePosition; } } }
private GyroControl _Seek(ShipControlCommons shipControl, Vector3D targetVector, Vector3D? targetUp, out double yawError, out double pitchError, out double rollError) { Vector3D referenceForward; Vector3D referenceLeft; Vector3D referenceUp; GyroControl gyroControl; // See if local orientation is the same as the ship if (shipControl.ShipUp == ShipUp && shipControl.ShipForward == ShipForward) { // Use same reference vectors and GyroControl referenceForward = shipControl.ReferenceForward; referenceLeft = shipControl.ReferenceLeft; referenceUp = shipControl.ReferenceUp; gyroControl = shipControl.GyroControl; } else { referenceForward = GetReferenceVector(shipControl, ShipForward); referenceLeft = GetReferenceVector(shipControl, ShipLeft); referenceUp = GetReferenceVector(shipControl, ShipUp); // Need our own GyroControl instance in this case gyroControl = new GyroControl(); gyroControl.Init(shipControl.Blocks, shipUp: ShipUp, shipForward: ShipForward); } // Determine projection of targetVector onto our reference unit vectors var dotZ = targetVector.Dot(referenceForward); var dotX = targetVector.Dot(referenceLeft); var dotY = targetVector.Dot(referenceUp); var projZ = dotZ * referenceForward; var projX = dotX * referenceLeft; var projY = dotY * referenceUp; // Determine yaw/pitch error by calculating angle between our forward // vector and targetVector var z = projZ.Length() * Math.Sign(dotZ); var x = projX.Length() * Math.Sign(dotX); var y = projY.Length() * Math.Sign(-dotY); // NB inverted yawError = Math.Atan2(x, z); pitchError = Math.Atan2(y, z); var gyroYaw = yawPID.Compute(yawError); var gyroPitch = pitchPID.Compute(pitchError); gyroControl.SetAxisVelocityFraction(GyroControl.Yaw, (float)gyroYaw); gyroControl.SetAxisVelocityFraction(GyroControl.Pitch, (float)gyroPitch); if (targetUp != null) { // Also adjust roll dotX = ((Vector3D)targetUp).Dot(referenceLeft); dotY = ((Vector3D)targetUp).Dot(referenceUp); projX = dotX * referenceLeft; projY = dotY * referenceUp; x = projX.Length() * Math.Sign(-dotX); y = projY.Length() * Math.Sign(dotY); rollError = Math.Atan2(x, y); var gyroRoll = rollPID.Compute(rollError); gyroControl.SetAxisVelocityFraction(GyroControl.Roll, (float)gyroRoll); } else { rollError = default(double); } return gyroControl; }
/* * States: * 0 -- Master Init * * * 160 Main Travel to target * * * *** below here are thruster-only routines (for now) *** ***170 Collision Detected From 160 ***Calculate collision avoidance ***then ->172 *** ***171 dummy state for debugging. ***172 do travel movemenet for collision avoidance. ***if arrive target, ->160 ***if secondary collision ->173 *** ***173 secondary collision ***if a type we can move around, try to move ->174 ***else go back to collision detection ->170 *** ***174 initilize escape plan ***->175 *** ***175 scan for an 'escape' route (pathfind) ***timeout of (default) 5 seconds ->MODE_ATTENTION ***after scans, ->180 *** ***180 travel to avoidance waypoint ***on arrival ->160 (main travel) ***on collision ->173 *** ***200 Arrived at target ***->MODE_ARRIVEDTARGET *** */ void doModeGoTarget() { StatusLog("clear", textPanelReport); StatusLog(moduleName + ":Going Target!", textPanelReport); StatusLog(moduleName + ":GT: current_state=" + current_state.ToString(), textPanelReport); // bWantFast = true; Echo("Going Target: state=" + current_state.ToString()); if (current_state == 0) { ResetTravelMovement(); if ((craft_operation & CRAFT_MODE_SLED) > 0) { bSled = true; if (shipSpeedMax > 45) { shipSpeedMax = 45; } } else { bSled = false; } if ((craft_operation & CRAFT_MODE_ROTOR) > 0) { bRotor = true; if (shipSpeedMax > 15) { shipSpeedMax = 15; } } else { bRotor = false; } GyroControl.SetRefBlock(shipOrientationBlock); double elevation = 0; ((IMyShipController)shipOrientationBlock).TryGetPlanetElevation(MyPlanetElevation.Surface, out elevation); if (bValidNavTarget) { if (elevation > shipDim.HeightInMeters()) { current_state = 150; } else { current_state = 160; } } else { setMode(MODE_ATTENTION); } bWantFast = true; } else if (current_state == 150) { //if (!bSled && !bRotor && dGravity > 0) if (dGravity > 0) { float fSaveAngle = minAngleRad; minAngleRad = 0.1f; bool bAligned = GyroMain(""); Echo("bAligned=" + bAligned.ToString()); minAngleRad = fSaveAngle; if (bAligned) { current_state = 160; } bWantFast = true; } else { current_state = 160; } } else if (current_state == 160) { // 160 move to Target Echo("Moving to Target"); Vector3D vTargetLocation = vNavTarget; Vector3D vVec = vTargetLocation - shipOrientationBlock.GetPosition(); double distance = vVec.Length(); Echo("distance=" + niceDoubleMeters(distance)); Echo("velocity=" + velocityShip.ToString("0.00")); StatusLog("clear", sledReport); StatusLog("Moving to Target\nD:" + niceDoubleMeters(distance) + " V:" + velocityShip.ToString(velocityFormat), sledReport); // Echo("TL:" + vTargetLocation.X.ToString("0.00") + ":" + vTargetLocation.Y.ToString("0.00") + ":" + vTargetLocation.Z.ToString("0.00")); // if(distance<17) // float range = RangeToNearestBase() + 100f + (float)velocityShip * 5f; // range = Math.Max(range, distance); // antennaMaxPower(false,range); if (bGoOption && (distance < arrivalDistanceMin)) { Echo("we have arrived"); if (NAVEmulateOld) { var tList = GetBlocksContains <IMyTerminalBlock>("NAV:"); for (int i1 = 0; i1 < tList.Count(); i1++) { // don't want to get blocks that have "NAV:" in customdata.. if (tList[i1].CustomName.StartsWith("NAV:")) { Echo("Found NAV: command:"); tList[i1].CustomName = "NAV: C Arrived Target"; } } } // bValidTargetLocation = false; // gyrosOff(); ResetMotion(); bValidNavTarget = false; // we used this one up. setMode(MODE_ARRIVEDTARGET); return; } // bool bYawOnly = false; // if (bSled || bRotor) bYawOnly = true; // debugGPSOutput("TargetLocation", vTargetLocation); bool bDoTravel = true; double elevation = 0; ((IMyShipController)shipOrientationBlock).TryGetPlanetElevation(MyPlanetElevation.Surface, out elevation); Echo("Elevation=" + elevation.ToString("0.0")); Echo("MinEle=" + NAVGravityMinElevation.ToString("0.0")); if (!bSled && !bRotor && NAVGravityMinElevation > 0 && elevation < NAVGravityMinElevation) { powerUpThrusters(thrustUpList, 100); // bDoTravel = false; } /* * if(!bSled && !bRotor && dGravity>0) * { * float fSaveAngle = minAngleRad; * minAngleRad = 0.1f; * bDoTravel = GyroMain(""); * Echo("Travel=" + bDoTravel.ToString()); * minAngleRad = fSaveAngle; * if (!bDoTravel) * bWantFast = true; * } */ if (bDoTravel) { Echo("Do Travel"); doTravelMovement(vTargetLocation, 3.0f, 200, 170); } } else if (current_state == 170) { // collision detection // IMyTextPanel tx = gpsPanel; // gpsPanel = textLongStatus; // StatusLog("clear", gpsPanel); bWantFast = true; Vector3D vTargetLocation = vNavTarget; ResetTravelMovement(); calcCollisionAvoid(vTargetLocation); // gpsPanel = tx; // current_state = 171; // testing current_state = 172; } else if (current_state == 171) { // just hold this state bWantFast = false; } else if (current_state == 172) { // Vector3D vVec = vAvoid - shipOrientationBlock.GetPosition(); // double distanceSQ = vVec.LengthSquared(); Echo("Collision Avoid"); StatusLog("clear", sledReport); StatusLog("Collision Avoid", sledReport); doTravelMovement(vAvoid, 5.0f, 160, 173); } else if (current_state == 173) { // secondary collision if (lastDetectedInfo.Type == MyDetectedEntityType.Asteroid || lastDetectedInfo.Type == MyDetectedEntityType.LargeGrid || lastDetectedInfo.Type == MyDetectedEntityType.SmallGrid ) { current_state = 174; } else { current_state = 170; // setMode(MODE_ATTENTION); } bWantFast = true; } else if (current_state == 174) { initEscapeScan(); ResetTravelMovement(); dtNavStartShip = DateTime.Now; current_state = 175; bWantFast = true; } else if (current_state == 175) { DateTime dtMaxWait = dtNavStartShip.AddSeconds(5.0f); DateTime dtNow = DateTime.Now; if (DateTime.Compare(dtNow, dtMaxWait) > 0) { setMode(MODE_ATTENTION); doTriggerMain(); return; } if (scanEscape()) { Echo("ESCAPE!"); current_state = 180; } bWantMedium = true; // bWantFast = true; } else if (current_state == 180) { doTravelMovement(vAvoid, 1f, 160, 173); } else if (current_state == 200) { // we have arrived at target StatusLog("clear", sledReport); StatusLog("Arrived at Target", sledReport); ResetMotion(); bValidNavTarget = false; // we used this one up. // float range = RangeToNearestBase() + 100f + (float)velocityShip * 5f; antennaMaxPower(false); sleepAllSensors(); setMode(MODE_ARRIVEDTARGET); if (NAVEmulateOld) { var tList = GetBlocksContains <IMyTerminalBlock>("NAV:"); for (int i1 = 0; i1 < tList.Count(); i1++) { // don't want to get blocks that have "NAV:" in customdata.. if (tList[i1].CustomName.StartsWith("NAV:")) { Echo("Found NAV: command:"); tList[i1].CustomName = "NAV: C Arrived Target"; } } } bWantFast = true; doTriggerMain(); } }
void Start() { m_gyro = m_GyroTransform.GetComponent<GyroControl>(); m_Text = m_TextTransform.GetComponent<Text>(); }
string doInit() { // initialization of each module goes here: // when all initialization is done, set init to true. // set autogyro defaults. LIMIT_GYROS = 1; minAngleRad = 0.09f; CTRL_COEFF = 0.75; /* * Log("Init:" + currentInit.ToString()); * double progress = currentInit * 100 / 3; * string sProgress = progressBar(progress); * StatusLog(sProgress, getTextBlock(sTextPanelReport)); */ Echo("Init"); if (currentInit == 0) { //StatusLog("clear",textLongStatus,true); StatusLog(DateTime.Now.ToString() + OurName + ":" + moduleName + ":INIT", textLongStatus, true); // if (!modeCommands.ContainsKey("launch")) modeCommands.Add("launch", MODE_LAUNCH); // if(!modeCommands.ContainsKey("godock")) modeCommands.Add("godock", MODE_DOCKING); sInitResults += initSerializeCommon(); Deserialize(); sInitResults += gridsInit(); sInitResults += BlockInit(); initLogging(); sInitResults += thrustersInit(gpsCenter); sInitResults += rotorsNavInit(); sInitResults += wheelsInit(gpsCenter); sInitResults += sensorInit(); // sInitResults += camerasensorsInit(gpsCenter); sInitResults += connectorsInit(); sInitResults += gyrosetup(); GyroControl.UpdateGyroList(gyros); GyroControl.SetRefBlock(gpsCenter); sInitResults += lightsInit(); initShipDim(); sInitResults += modeOnInit(); // handle mode initializting from load/recompile.. init = true; } currentInit++; if (init) { currentInit = 0; bWantFast = false; } Log(sInitResults); return(sInitResults); }
void Start() { gyroControl = gyroControlObj.GetComponent <GyroControl> (); }
string doInit() { // Echo("InitA:" + currentInit + ":"+Runtime.CurrentInstructionCount+ "/"+Runtime.MaxInstructionCount); // initialization of each module goes here: // when all initialization is done, set init to true. Log("Init:" + currentInit.ToString()); Echo(gtsAllBlocks.Count.ToString() + " Blocks"); /* * double progress = currentInit * 100 / 3; // 3=Number of expected INIT phases. * string sProgress = progressBar(progress); * StatusLog(moduleName + sProgress, textPanelReport); */ if (shipOrientationBlock != null) { // anchorPosition = shipOrientationBlock; // currentPosition = anchorPosition.GetPosition(); } // Echo("InitB:" + currentInit + ":"+Runtime.CurrentInstructionCount+ "/"+Runtime.MaxInstructionCount); if (currentInit == 0) { // Echo("Init0:" + currentInit + ":"+Runtime.CurrentInstructionCount+ "/"+Runtime.MaxInstructionCount); // StatusLog("clear", textLongStatus, true); // only MAIN module should clear long status on init. StatusLog(DateTime.Now.ToString() + " " + OurName + ":" + moduleName + ":INIT", textLongStatus, true); /* * if(!modeCommands.ContainsKey("launchprep")) modeCommands.Add("launchprep", MODE_LAUNCHPREP); */ sInitResults += gridsInit(); initLogging(); initTimers(); sInitResults += SerializeInit(); Deserialize(); // get info from savefile to avoid blind-rewrite of (our) defaults } else if (currentInit == 1) { Deserialize();// get info from savefile to avoid blind-rewrite of (our) defaults sInitResults += DefaultOrientationBlockInit(); initCargoCheck(); initPower(); sInitResults += thrustersInit(shipOrientationBlock); sInitResults += gyrosetup(); GyroControl.UpdateGyroList(gyros); if (gtsAllBlocks.Count < 300) { currentInit = 2; // go ahead and do next step. } } if (currentInit == 2) { sInitResults += wheelsInit(shipOrientationBlock); sInitResults += rotorsNavInit(); sInitResults += wheelsInit(shipOrientationBlock); sInitResults += sensorInit(true); if (gtsAllBlocks.Count < 100) { currentInit = 3; // go ahead and do next step. } } if (currentInit == 3) { sInitResults += connectorsInit(); sInitResults += tanksInit(); sInitResults += drillInit(); sInitResults += controllersInit(); if (gtsAllBlocks.Count < 100) { currentInit = 4; // go ahead and do next step. } } if (currentInit == 4) { sInitResults += ejectorsInit(); sInitResults += antennaInit(); sInitResults += gasgenInit(); sInitResults += camerasensorsInit(shipOrientationBlock); sInitResults += airventInit(); // Serialize(); autoConfig(); bWantFast = false; if (bGotAntennaName) { sBanner = "*" + OurName + ":" + moduleName + " V" + sVersion + " "; } if (sBanner.Length > 34) { sBanner = OurName + ":" + moduleName + "\nV" + sVersion + " "; } if (shipOrientationBlock is IMyShipController) { MyShipMass myMass; myMass = ((IMyShipController)shipOrientationBlock).CalculateShipMass(); gridBaseMass = myMass.BaseMass; }//// initShipDim(); sInitResults += modeOnInit(); // handle mode initializing from load/recompile.. init = true; // we are done } currentInit++; if (init) { currentInit = 0; } Log(sInitResults); return(sInitResults); }
void Start() { m_gyro = m_GyroTransform.GetComponent <GyroControl>(); m_Text = m_TextTransform.GetComponent <Text>(); }
string doInit() { // initialization of each module goes here: // when all initialization is done, set init to true. /* * if(currentInit==0) initLogging(); * * // Log("Init:" + currentInit.ToString()); * double progress = currentInit * 100 / 3; * string sProgress = progressBar(progress); * StatusLog(moduleName + sProgress, textPanelReport); */ do { // echoInstructions("Init:" + currentInit + " | "); switch (currentInit) { case 0: sInitResults += gridsInit(); break; case 1: /* * add commands to set modes * For Example: * if(!modeCommands.ContainsKey("launchprep")) modeCommands.Add("launchprep", MODE_LAUNCHPREP); */ modeCommands.Clear(); // if (!modeCommands.ContainsKey("findore")) modeCommands.Add("findore", MODE_FINDORE); // if (!modeCommands.ContainsKey("doscan")) modeCommands.Add("doscan", MODE_DOSCAN); if (!modeCommands.ContainsKey("mine")) { modeCommands.Add("mine", MODE_MINE); } if (!modeCommands.ContainsKey("bore")) { modeCommands.Add("bore", MODE_BORESINGLE); } break; case 2: initLogging(); break; case 3: StatusLog(DateTime.Now.ToString() + " " + OurName + ":" + moduleName + ":INIT", textLongStatus, true); break; case 4: sInitResults += SerializeInit(); Deserialize(); break; case 5: break; case 6: sInitResults += DefaultOrientationBlockInit(); break; case 7: initShipDim(shipOrientationBlock); break; case 8: sInitResults += connectorsInit(); break; case 9: sInitResults += thrustersInit(shipOrientationBlock); break; case 10: sInitResults += camerasensorsInit(shipOrientationBlock); break; case 11: sInitResults += SensorInit(shipOrientationBlock); break; case 12: sInitResults += tanksInit(); break; case 13: sInitResults += gyrosetup(); break; case 14: GyroControl.UpdateGyroList(gyros); break; case 15: sInitResults += drillInit(); break; case 16: sInitResults += ejectorsInit(); break; case 17: initCargoCheck(); break; case 18: initAsteroidsInfo(); break; case 19: // Deserialize(); break; case 20: initOreLocInfo(); break; case 21: initPower(); break; case 22: tanksInit(); break; case 23: sInitResults += modeOnInit(); break; case 24: if (sensorsList.Count < 2) { // bStartupError = true; sStartupError += "\nNot enough Sensors detected!"; } if (!HasDrills()) { // bStartupError = true; sStartupError += "\nNo Drills found!"; } break; case 25: Serialize(); init = true; break; } currentInit++; // echoInstructions("EInit:" + currentInit + " | "); Echo("%=" + (float)Runtime.CurrentInstructionCount / (float)Runtime.MaxInstructionCount); }while (!init && (((float)Runtime.CurrentInstructionCount / (float)Runtime.MaxInstructionCount) < 0.5f)); if (init) { currentInit = 0; } // Log(sInitResults); Echo("Init exit"); return(sInitResults); }
void Start() { gridSystem = GridTerminalSystem; gridId = Me.CubeGrid.EntityId; turrets = GetBlocks <IMyLargeTurretBase>(turretGroup, useSubgrids); wcTurrets = new WcPbApi(); try { if (!wcTurrets.Activate(Me)) { wcTurrets = null; } } catch { wcTurrets = null; } if (string.IsNullOrWhiteSpace(rcName)) { rc = GetBlock <IMyRemoteControl>(); } else { rc = GetBlock <IMyRemoteControl>(rcName, useSubgrids); } if (rc == null) { throw new Exception("Remote control block not found."); } gyros = new GyroControl(rc); origin = rc.GetPosition(); this.guns = new List <IMyUserControllableGun>(); List <IMyUserControllableGun> guns = GetBlocks <IMyUserControllableGun>(gunsGroup, useSubgrids); foreach (IMyUserControllableGun g in guns) { if (!(g is IMyLargeTurretBase)) { g.SetValueBool("Shoot", false); if (rocketMode != RocketMode.None) { IMySmallMissileLauncher m = g as IMySmallMissileLauncher; if (m != null) { rockets.Add(m); continue; } } this.guns.Add(g); } } if (receiveHelpMsgs) { helpListener = IGC.RegisterBroadcastListener("AttackDrone_" + commsId); helpListener.SetMessageCallback(""); } thrust = new ThrusterControl(rc, GetBlocks <IMyThrust>(thrustGroup)); maxAngle *= Math.PI / 180; startRuntime = -1; }
private GyroControl _Seek(ShipControlCommons shipControl, Vector3D targetVector, Vector3D?targetUp, out double yawPitchError, out double rollError) { var angularVelocity = shipControl.AngularVelocity; if (angularVelocity == null) { // No ship controller, no action yawPitchError = rollError = Math.PI; return(shipControl.GyroControl); } Vector3D referenceForward; Vector3D referenceLeft; Vector3D referenceUp; GyroControl gyroControl; // See if local orientation is the same as the ship if (shipControl.ShipUp == ShipUp && shipControl.ShipForward == ShipForward) { // Use same reference vectors and GyroControl referenceForward = shipControl.ReferenceForward; referenceLeft = shipControl.ReferenceLeft; referenceUp = shipControl.ReferenceUp; gyroControl = shipControl.GyroControl; } else { referenceForward = GetReferenceVector(shipControl, ShipForward); referenceLeft = GetReferenceVector(shipControl, ShipLeft); referenceUp = GetReferenceVector(shipControl, ShipUp); // Need our own GyroControl instance in this case gyroControl = new GyroControl(); gyroControl.Init(shipControl.Blocks, shipUp: ShipUp, shipForward: ShipForward); } targetVector = Vector3D.Normalize(targetVector); // Invert our world matrix var toLocal = MatrixD.Transpose(MatrixD.CreateWorld(Vector3D.Zero, referenceForward, referenceUp)); // And bring targetVector & angular velocity into local space var localTarget = Vector3D.TransformNormal(-targetVector, toLocal); var localVel = Vector3D.TransformNormal((Vector3D)angularVelocity, toLocal); // Use simple trig to get the error angles var yawError = Math.Atan2(localTarget.X, localTarget.Z); var pitchError = Math.Atan2(localTarget.Y, localTarget.Z); // Set desired angular velocity var desiredYawVel = yawPID.Compute(yawError); var desiredPitchVel = pitchPID.Compute(pitchError); //shipControl.Echo(string.Format("desiredVel = {0:F3}, {1:F3}", desiredYawVel, desiredPitchVel)); // Translate to gyro outputs double gyroYaw = 0.0; if (Math.Abs(desiredYawVel) >= ControlThreshold) { gyroYaw = yawVPID.Compute(desiredYawVel - localVel.X); } double gyroPitch = 0.0; if (Math.Abs(desiredPitchVel) >= ControlThreshold) { gyroPitch = pitchVPID.Compute(desiredPitchVel - localVel.Y); } //shipControl.Echo(string.Format("yaw, pitch = {0:F3}, {1:F3}", gyroYaw, gyroPitch)); gyroControl.SetAxisVelocity(GyroControl.Yaw, (float)gyroYaw); gyroControl.SetAxisVelocity(GyroControl.Pitch, (float)gyroPitch); // Determine total yaw/pitch error yawPitchError = Math.Acos(MathHelperD.Clamp(Vector3D.Dot(targetVector, referenceForward), -1.0, 1.0)); if (targetUp != null) { // Also adjust roll by rotating targetUp vector localTarget = Vector3D.TransformNormal((Vector3D)targetUp, toLocal); rollError = Math.Atan2(localTarget.X, localTarget.Y); var desiredRollVel = rollPID.Compute(rollError); //shipControl.Echo(string.Format("desiredRollVel = {0:F3}", desiredRollVel)); double gyroRoll = 0.0; if (Math.Abs(desiredRollVel) >= ControlThreshold) { gyroRoll = rollVPID.Compute(desiredRollVel - localVel.Z); } //shipControl.Echo(string.Format("roll = {0:F3}", gyroRoll)); gyroControl.SetAxisVelocity(GyroControl.Roll, (float)gyroRoll); // Only care about absolute error rollError = Math.Abs(rollError); } else { rollError = 0.0; } return(gyroControl); }
public void insertData(GyroControl gc) { Gyroscope gyro = gc.gyro; Vector3 linearAcc = gyro.userAcceleration; Vector3 gyroRotationSpeed = gyro.rotationRateUnbiased; //Vector3 linearAcc = new Vector3(0.1f, 0.1f, 0.2f); //Vector3 gyroRotationSpeed = new Vector3(0.1f, 0.1f, 0.2f); float linearAccMagnitude = linearAcc.magnitude; float gyroRotationSpeedMagnitude = gyroRotationSpeed.magnitude; Vector3 linearAccClearedFromConstant = new Vector3(0, 0, 0); Vector3 gyroRotationSpeedClearedFromConstant = new Vector3(0, 0, 0); Vector3 linearAccAverage = new Vector3(0, 0, 0); float linearAccMagnitudeClearedFromConstant = 0; float gyroRotationSpeedMagnitudeClearedFromConstant = 0; float linearAccEnergy = 0; float gyroRotationEnergy = 0; int N = dataQueue.Count; if (N == 0) { N++; } foreach (MovementStepData md in dataQueue) { linearAccClearedFromConstant += md.linearAcc; linearAccAverage += md.linearAcc; gyroRotationSpeedClearedFromConstant += md.gyroRotationSpeed; linearAccMagnitudeClearedFromConstant += md.linearAccMagnitude; gyroRotationSpeedMagnitudeClearedFromConstant += md.gyroRotationSpeedMagnitude; linearAccEnergy += md.linearAccMagnitudeClearedFromConstant; gyroRotationEnergy += md.gyroRotationSpeedMagnitudeClearedFromConstant; } linearAccClearedFromConstant = linearAcc - 1 / N * linearAccClearedFromConstant; linearAccAverage /= N; gyroRotationSpeedClearedFromConstant = gyroRotationSpeed - 1 / N * gyroRotationSpeedClearedFromConstant; linearAccMagnitudeClearedFromConstant = linearAccMagnitude - 1 / N * linearAccMagnitudeClearedFromConstant; gyroRotationSpeedMagnitudeClearedFromConstant = gyroRotationSpeedMagnitude - 1 / N * gyroRotationSpeedMagnitudeClearedFromConstant; linearAccEnergy /= N; gyroRotationEnergy /= N; float linearAccVariance = 0; float gyroRotationVariance = 0; foreach (MovementStepData md in dataQueue) { float curLinVarMember = md.linearAccMagnitudeClearedFromConstant + linearAccEnergy; linearAccVariance += curLinVarMember * curLinVarMember; float curRotVarMember = md.gyroRotationSpeedMagnitudeClearedFromConstant + gyroRotationEnergy; gyroRotationVariance += curRotVarMember * curRotVarMember; } linearAccVariance /= N; gyroRotationVariance /= N; MovementStepData mdNew = new MovementStepData(); mdNew.gyroRotationEnergy = gyroRotationEnergy; mdNew.gyroRotationSpeed = gyroRotationSpeed; mdNew.gyroRotationSpeedClearedFromConstant = gyroRotationSpeedClearedFromConstant; mdNew.gyroRotationSpeedMagnitude = gyroRotationSpeedMagnitude; mdNew.gyroRotationSpeedMagnitudeClearedFromConstant = gyroRotationSpeedMagnitudeClearedFromConstant; mdNew.gyroRotationVariance = gyroRotationVariance; mdNew.linearAcc = linearAcc; mdNew.linearAccClearedFromConstant = linearAccClearedFromConstant; mdNew.linearAccEnergy = linearAccEnergy; mdNew.linearAccMagnitude = linearAccMagnitude; mdNew.linearAccMagnitudeClearedFromConstant = linearAccMagnitudeClearedFromConstant; mdNew.linearAccVariance = linearAccVariance; mdNew.linearAccAverage = linearAccAverage; if (N >= maxDepth) { dataQueue.Dequeue(); } dataQueue.Enqueue(mdNew); lastMSD = mdNew; }
private GyroControl _Seek(ShipControlCommons shipControl, Vector3D targetVector, Vector3D?targetUp, out double yawError, out double pitchError, out double rollError) { Vector3D referenceForward; Vector3D referenceLeft; Vector3D referenceUp; GyroControl gyroControl; // See if local orientation is the same as the ship if (shipControl.ShipUp == ShipUp && shipControl.ShipForward == ShipForward) { // Use same reference vectors and GyroControl referenceForward = shipControl.ReferenceForward; referenceLeft = shipControl.ReferenceLeft; referenceUp = shipControl.ReferenceUp; gyroControl = shipControl.GyroControl; } else { referenceForward = GetReferenceVector(shipControl, ShipForward); referenceLeft = GetReferenceVector(shipControl, ShipLeft); referenceUp = GetReferenceVector(shipControl, ShipUp); // Need our own GyroControl instance in this case gyroControl = new GyroControl(); gyroControl.Init(shipControl.Blocks, shipUp: ShipUp, shipForward: ShipForward); } // Determine projection of targetVector onto our reference unit vectors var dotZ = targetVector.Dot(referenceForward); var dotX = targetVector.Dot(referenceLeft); var dotY = targetVector.Dot(referenceUp); var projZ = dotZ * referenceForward; var projX = dotX * referenceLeft; var projY = dotY * referenceUp; // Determine yaw/pitch error by calculating angle between our forward // vector and targetVector var z = projZ.Length() * Math.Sign(dotZ); var x = projX.Length() * Math.Sign(dotX); var y = projY.Length() * Math.Sign(-dotY); // NB inverted yawError = Math.Atan2(x, z); pitchError = Math.Atan2(y, z); var gyroYaw = yawPID.Compute(yawError); var gyroPitch = pitchPID.Compute(pitchError); gyroControl.SetAxisVelocityFraction(GyroControl.Yaw, (float)gyroYaw); gyroControl.SetAxisVelocityFraction(GyroControl.Pitch, (float)gyroPitch); if (targetUp != null) { // Also adjust roll dotX = ((Vector3D)targetUp).Dot(referenceLeft); dotY = ((Vector3D)targetUp).Dot(referenceUp); projX = dotX * referenceLeft; projY = dotY * referenceUp; x = projX.Length() * Math.Sign(-dotX); y = projY.Length() * Math.Sign(dotY); rollError = Math.Atan2(x, y); var gyroRoll = rollPID.Compute(rollError); gyroControl.SetAxisVelocityFraction(GyroControl.Roll, (float)gyroRoll); } else { rollError = default(double); } return(gyroControl); }
string doInit() { // initialization of each module goes here: // when all initialization is done, set init to true. // set autogyro defaults. // LIMIT_GYROS = 1; // minAngleRad = 0.09f; // CTRL_COEFF = 0.75; // TODO: change to more atomic init Echo("Init:" + currentInit); if (currentInit == 0) { //StatusLog("clear",textLongStatus,true); StatusLog(DateTime.Now.ToString() + OurName + ":" + moduleName + ":INIT", textLongStatus, true); // if (!modeCommands.ContainsKey("launch")) modeCommands.Add("launch", MODE_LAUNCH); // if(!modeCommands.ContainsKey("godock")) modeCommands.Add("godock", MODE_DOCKING); if (!modeCommands.ContainsKey("scantest")) { modeCommands.Add("scantest", MODE_SCANTEST); } sInitResults += SerializeInit(); Deserialize(); sInitResults += gridsInit(); sInitResults += DefaultOrientationBlockInit(); initLogging(); } else if (currentInit == 1) { sInitResults += thrustersInit(shipOrientationBlock); sInitResults += rotorsNavInit(); sInitResults += wheelsInit(shipOrientationBlock); sInitResults += SensorInit(shipOrientationBlock); sInitResults += camerasensorsInit(shipOrientationBlock); sInitResults += connectorsInit(); } else if (currentInit == 2) { sInitResults += gyrosetup(); GyroControl.UpdateGyroList(gyros); GyroControl.SetRefBlock(shipOrientationBlock); sInitResults += lightsInit(); sInitResults += camerasensorsInit(shipOrientationBlock); calculateGridBBPosition(shipOrientationBlock); initShipDim(shipOrientationBlock); sInitResults += modeOnInit(); // handle mode initializting from load/recompile.. init = true; } currentInit++; if (init) { currentInit = 0; // bWantFast = false; } Log(sInitResults); // Echo("XXInit:"+currentInit); return(sInitResults); }
void doModeGoTarget() { StatusLog("clear", textPanelReport); StatusLog(moduleName + ":Going Target!", textPanelReport); // StatusLog(moduleName + ":GT: current_state=" + current_state.ToString(), textPanelReport); // bWantFast = true; Echo("Going Target: state=" + current_state.ToString()); if (NAVTargetName != "") { Echo(NAVTargetName); } string sNavDebug = ""; sNavDebug += "GT:S=" + current_state; // sNavDebug += " MinE=" + NAVGravityMinElevation; // ResetMotion(); if (current_state == 0) { ResetTravelMovement(); // sStartupError+="\nStart movemenet: ArrivalMode="+NAVArrivalMode+" State="+NAVArrivalState; if ((craft_operation & CRAFT_MODE_SLED) > 0) { bSled = true; if (shipSpeedMax > 45) { shipSpeedMax = 45; } } else { bSled = false; } if ((craft_operation & CRAFT_MODE_ROTOR) > 0) { bRotor = true; if (shipSpeedMax > 15) { shipSpeedMax = 15; } } else { bRotor = false; } if ((craft_operation & CRAFT_MODE_WHEEL) > 0) { bWheels = true; // if (shipSpeedMax > 15) shipSpeedMax = 15; } else { bWheels = false; } GyroControl.SetRefBlock(shipOrientationBlock); // TODO: Put a timer on this so it's not done Update1 double elevation = 0; ((IMyShipController)shipOrientationBlock).TryGetPlanetElevation(MyPlanetElevation.Surface, out elevation); if (!bSled && !bRotor) { // if flying ship // make sure set to default if (NAVGravityMinElevation < 0) { NAVGravityMinElevation = 75; // for EFM getting to target 'arrived' radius } // NAVGravityMinElevation = (float)shipSpeedMax*2.5f; } if (bValidNavTarget) { if (elevation > shipDim.HeightInMeters()) { current_state = 150; } else { current_state = 160; } } else { setMode(MODE_ATTENTION); } bWantFast = true; } else if (current_state == 150) { bWantFast = true; if (dGravity > 0) { double elevation = 0; ((IMyShipController)shipOrientationBlock).TryGetPlanetElevation(MyPlanetElevation.Surface, out elevation); sNavDebug += " E=" + elevation.ToString("0.0"); float fSaveAngle = minAngleRad; minAngleRad = 0.1f; Vector3D grav = (shipOrientationBlock as IMyShipController).GetNaturalGravity(); bool bAligned = GyroMain("", grav, shipOrientationBlock); sNavDebug += " Aligned=" + bAligned.ToString(); Echo("bAligned=" + bAligned.ToString()); minAngleRad = fSaveAngle; if (bAligned || elevation < shipDim.HeightInMeters() * 2) { gyrosOff(); if (NAVGravityMinElevation > 0) { current_state = 155; } else { current_state = 160; } } } else { current_state = 160; } } else if (current_state == 151) { bWantFast = true; if (dGravity > 0 || btmWheels) { double elevation = 0; ((IMyShipController)shipOrientationBlock).TryGetPlanetElevation(MyPlanetElevation.Surface, out elevation); sNavDebug += " E=" + elevation.ToString("0.0"); float fSaveAngle = minAngleRad; minAngleRad = 0.1f; Vector3D grav = (shipOrientationBlock as IMyShipController).GetNaturalGravity(); bool bAligned = GyroMain("", grav, shipOrientationBlock); sNavDebug += " Aligned=" + bAligned.ToString(); Echo("bAligned=" + bAligned.ToString()); minAngleRad = fSaveAngle; if (bAligned || elevation < shipDim.HeightInMeters() * 2) { gyrosOff(); if (NAVGravityMinElevation > 0) { current_state = 155; } else { current_state = 160; } } else { current_state = 150; // try again to be aligned. } } else { current_state = 160; } } else if (current_state == 155) { // for use in gravity: aim at location using yaw only bWantFast = true; if (bWheels) { current_state = 160; return; } if (dGravity > 0) { Vector3D grav = (shipOrientationBlock as IMyShipController).GetNaturalGravity(); bool bAligned = GyroMain("", grav, shipOrientationBlock); sNavDebug += " Aligned=" + bAligned.ToString(); double yawangle = -999; yawangle = CalculateYaw(vNavTarget, shipOrientationBlock); bool bAimed = Math.Abs(yawangle) < 0.1; // NOTE: 2x allowance Echo("yawangle=" + yawangle.ToString()); sNavDebug += " Yaw=" + yawangle.ToString("0.00"); if (!bAimed) { if (btmRotor) { Echo("Rotor"); DoRotorRotate(yawangle); } else // use for both sled and flight { DoRotate(yawangle, "Yaw"); } } if (bAligned && bAimed) { gyrosOff(); current_state = 160; } else if (bAligned && Math.Abs(yawangle) < 0.5) { float atmo; float hydro; float ion; calculateHoverThrust(thrustForwardList, out atmo, out hydro, out ion); atmo += 1; hydro += 1; ion += 1; powerUpThrusters(thrustForwardList, atmo, thrustatmo); powerUpThrusters(thrustForwardList, hydro, thrusthydro); powerUpThrusters(thrustForwardList, ion, thrustion); } else { powerDownThrusters(thrustForwardList); } } else { current_state = 160; } } else if (current_state == 156) { // realign gravity bWantFast = true; Vector3D grav = (shipOrientationBlock as IMyShipController).GetNaturalGravity(); bool bAimed = GyroMain("", grav, shipOrientationBlock); if (bAimed) { gyrosOff(); current_state = 160; } } else if (current_state == 160) { // 160 move to Target Echo("Moving to Target"); Vector3D vTargetLocation = vNavTarget; Vector3D vVec = vTargetLocation - shipOrientationBlock.GetPosition(); double distance = vVec.Length(); Echo("distance=" + niceDoubleMeters(distance)); Echo("velocity=" + velocityShip.ToString("0.00")); StatusLog("clear", sledReport); string sTarget = "Moving to Target"; if (NAVTargetName != "") { sTarget = "Moving to " + NAVTargetName; } StatusLog(sTarget + "\nD:" + niceDoubleMeters(distance) + " V:" + velocityShip.ToString(velocityFormat), sledReport); StatusLog(sTarget + "\nDistance: " + niceDoubleMeters(distance) + "\nVelocity: " + niceDoubleMeters(velocityShip) + "/s", textPanelReport); if (bGoOption && (distance < arrivalDistanceMin)) { current_state = 500; Echo("we have arrived"); bWantFast = true; return; } // debugGPSOutput("TargetLocation", vTargetLocation); bool bDoTravel = true; if (NAVGravityMinElevation > 0 && dGravity > 0) { double elevation = 0; MyShipVelocities mysSV = ((IMyShipController)shipOrientationBlock).GetShipVelocities(); Vector3D lv = mysSV.LinearVelocity; var upVec = ((IMyShipController)shipOrientationBlock).WorldMatrix.Up; var vertVel = Vector3D.Dot(lv, upVec); // Echo("LV=" + Vector3DToString(lv)); // sNavDebug += " LV=" + Vector3DToString(lv); // sNavDebug += " vertVel=" + vertVel.ToString("0.0"); // sNavDebug += " Hvel=" + lv.Y.ToString("0.0"); // NOTE: Elevation is only updated by game every 30? ticks. so it can be WAY out of date based on movement ((IMyShipController)shipOrientationBlock).TryGetPlanetElevation(MyPlanetElevation.Surface, out elevation); sNavDebug += " E=" + elevation.ToString("0.0"); sNavDebug += " V=" + velocityShip.ToString("0.00"); Echo("Elevation=" + elevation.ToString("0.0")); Echo("MinEle=" + NAVGravityMinElevation.ToString("0.0")); // double stopD = calculateStoppingDistance(thrustUpList, velocityShip, dGravity); double stopD = 0; if (vertVel < 0) { stopD = calculateStoppingDistance(thrustUpList, Math.Abs(vertVel), dGravity); } double maxStopD = calculateStoppingDistance(thrustUpList, fMaxWorldMps, dGravity); float atmo; float hydro; float ion; calculateHoverThrust(thrustUpList, out atmo, out hydro, out ion); // sNavDebug += " SD=" + stopD.ToString("0"); if ( // !bSled && !bRotor && NAVGravityMinElevation > 0) { if ( vertVel < -0.5 && // we are going downwards (elevation - stopD * 2) < NAVGravityMinElevation) { // too low. go higher // Emergency thrust sNavDebug += " EM UP!"; Vector3D grav = (shipOrientationBlock as IMyShipController).GetNaturalGravity(); bool bAligned = GyroMain("", grav, shipOrientationBlock); powerUpThrusters(thrustUpList, 100); bDoTravel = false; bWantFast = true; } else if (elevation < NAVGravityMinElevation) { // push upwards atmo += Math.Min(5f, (float)shipSpeedMax); hydro += Math.Min(5f, (float)shipSpeedMax); ion += Math.Min(5f, (float)shipSpeedMax); sNavDebug += " UP! A" + atmo.ToString("0.00"); // + " H"+hydro.ToString("0.00") + " I"+ion.ToString("0.00"); //powerUpThrusters(thrustUpList, 100); powerUpThrusters(thrustUpList, atmo, thrustatmo); powerUpThrusters(thrustUpList, hydro, thrusthydro); powerUpThrusters(thrustUpList, ion, thrustion); } else if (elevation > (maxStopD + NAVGravityMinElevation * 1.25)) { // if we are higher than maximum possible stopping distance, go down fast. sNavDebug += " SUPERHIGH"; // Vector3D grav = (shipOrientationBlock as IMyShipController).GetNaturalGravity(); // bool bAligned = GyroMain("", grav, shipOrientationBlock); powerDownThrusters(thrustUpList, thrustAll, true); Vector3D grav = (shipOrientationBlock as IMyShipController).GetNaturalGravity(); bool bAligned = GyroMain("", grav, shipOrientationBlock); if (!bAligned) { bWantFast = true; bDoTravel = false; } // powerUpThrusters(thrustUpList, 1f); } else if ( elevation > NAVGravityMinElevation * 2 // too high // && ((elevation-stopD)>NAVGravityMinElevation) // we can stop in time. // && velocityShip < shipSpeedMax * 1.1 // to fast in any direction // && Math.Abs(lv.X) < Math.Min(25, shipSpeedMax) // not too fast // && Math.Abs(lv.Y) < Math.Min(25, shipSpeedMax) // not too fast downwards (or upwards) ) { // too high sNavDebug += " HIGH"; //DOWN! A" + atmo.ToString("0.00");// + " H" + hydro.ToString("0.00") + " I" + ion.ToString("0.00"); if (vertVel > 2) // going up { // turn off thrusters. sNavDebug += " ^"; powerDownThrusters(thrustUpList, thrustAll, true); } else if (vertVel < -0.5) // going down { sNavDebug += " v"; if (vertVel > (-Math.Min(15, shipSpeedMax))) { // currently descending at less than desired atmo -= Math.Max(25f, Math.Min(5f, (float)velocityShip / 2)); hydro -= Math.Max(25f, Math.Min(5f, (float)velocityShip / 2)); ion -= Math.Max(25f, Math.Min(5f, (float)velocityShip / 2)); sNavDebug += " DOWN! A" + atmo.ToString("0.00");// + " H" + hydro.ToString("0.00") + " I" + ion.ToString("0.00"); // bDoTravel = false; } else { // we are descending too fast. atmo += Math.Max(100f, Math.Min(5f, (float)velocityShip / 2)); hydro += Math.Max(100f, Math.Min(5f, (float)velocityShip / 2)); ion += Math.Max(100f, Math.Min(5f, (float)velocityShip / 2)); sNavDebug += " 2FAST! A" + atmo.ToString("0.00");// + " H" + hydro.ToString("0.00") + " I" + ion.ToString("0.00"); Vector3D grav = (shipOrientationBlock as IMyShipController).GetNaturalGravity(); bool bAligned = GyroMain("", grav, shipOrientationBlock); if (!bAligned) { bWantFast = true; bDoTravel = false; } // bDoTravel = false; } } else { sNavDebug += " -"; atmo -= 5; hydro -= 5; ion -= 5; } powerUpThrusters(thrustUpList, atmo, thrustatmo); powerUpThrusters(thrustUpList, hydro, thrusthydro); powerUpThrusters(thrustUpList, ion, thrustion); } else { // normal hover powerDownThrusters(thrustUpList); } } } if (bDoTravel) { Echo("Do Travel"); doTravelMovement(vTargetLocation, (float)arrivalDistanceMin, 500, 300); } else { powerDownThrusters(thrustForwardList); } } else if (current_state == 300) { // collision detection bWantFast = true; Vector3D vTargetLocation = vNavTarget; ResetTravelMovement(); calcCollisionAvoid(vTargetLocation); // current_state = 301; // testing current_state = 320; } else if (current_state == 301) { // just hold this state bWantFast = false; } else if (current_state == 320) { // Vector3D vVec = vAvoid - shipOrientationBlock.GetPosition(); // double distanceSQ = vVec.LengthSquared(); Echo("Primary Collision Avoid"); StatusLog("clear", sledReport); StatusLog("Collision Avoid", sledReport); StatusLog("Collision Avoid", textPanelReport); doTravelMovement(vAvoid, 5.0f, 160, 340); } else if (current_state == 340) { // secondary collision if ( lastDetectedInfo.Type == MyDetectedEntityType.LargeGrid || lastDetectedInfo.Type == MyDetectedEntityType.SmallGrid ) { current_state = 345; } else if (lastDetectedInfo.Type == MyDetectedEntityType.Asteroid ) { current_state = 350; } else { current_state = 300; // setMode(MODE_ATTENTION); } bWantFast = true; } else if (current_state == 345) { // we hit a grid. align to it Vector3D[] corners = new Vector3D[BoundingBoxD.CornerCount]; BoundingBoxD bbd = lastDetectedInfo.BoundingBox; bbd.GetCorners(corners); GridUpVector = PlanarNormal(corners[3], corners[4], corners[7]); GridRightVector = PlanarNormal(corners[0], corners[1], corners[4]); bWantFast = true; current_state = 348; } else if (current_state == 348) { bWantFast = true; if (GyroMain("up", GridUpVector, shipOrientationBlock)) { current_state = 349; } } else if (current_state == 349) { bWantFast = true; if (GyroMain("right", GridRightVector, shipOrientationBlock)) { current_state = 350; } } else if (current_state == 350) { // initEscapeScan(bCollisionWasSensor, !bCollisionWasSensor); initEscapeScan(bCollisionWasSensor); ResetTravelMovement(); dtNavStartShip = DateTime.Now; current_state = 360; bWantFast = true; } else if (current_state == 360) { StatusLog("Collision Avoid\nScan for escape route", textPanelReport); DateTime dtMaxWait = dtNavStartShip.AddSeconds(5.0f); DateTime dtNow = DateTime.Now; if (DateTime.Compare(dtNow, dtMaxWait) > 0) { setMode(MODE_ATTENTION); doTriggerMain(); return; } if (scanEscape()) { Echo("ESCAPE!"); current_state = 380; } bWantMedium = true; // bWantFast = true; } else if (current_state == 380) { StatusLog("Collision Avoid Travel", textPanelReport); Echo("Escape Collision Avoid"); doTravelMovement(vAvoid, 1f, 160, 340); } else if (current_state == 500) { // we have arrived at target /* * // check for more nav commands * if(wicoNavCommands.Count>0) * { * wicoNavCommands.RemoveAt(0); * } * if(wicoNavCommands.Count>0) * { * // another command * wicoNavCommandProcessNext(); * } * else */ { StatusLog("clear", sledReport); StatusLog("Arrived at Target", sledReport); StatusLog("Arrived at Target", textPanelReport); sNavDebug += " ARRIVED!"; ResetMotion(); bValidNavTarget = false; // we used this one up. // float range = RangeToNearestBase() + 100f + (float)velocityShip * 5f; antennaMaxPower(false); SensorsSleepAll(); // sStartupError += "Finish WP:" + wicoNavCommands.Count.ToString()+":"+NAVArrivalMode.ToString(); // set to desired mode and state setMode(NAVArrivalMode); current_state = NAVArrivalState; // set up defaults for next run (in case they had been changed) NAVArrivalMode = MODE_ARRIVEDTARGET; NAVArrivalState = 0; NAVTargetName = ""; bGoOption = true; // setMode(MODE_ARRIVEDTARGET); if (NAVEmulateOld) { var tList = GetBlocksContains <IMyTerminalBlock>("NAV:"); for (int i1 = 0; i1 < tList.Count(); i1++) { // don't want to get blocks that have "NAV:" in customdata.. if (tList[i1].CustomName.StartsWith("NAV:")) { Echo("Found NAV: command:"); tList[i1].CustomName = "NAV: C Arrived Target"; } } } } bWantFast = true; doTriggerMain(); } NavDebug(sNavDebug); }
void doModeGoTarget() { StatusLog("clear", textPanelReport); StatusLog(moduleName + ":Going Target!", textPanelReport); StatusLog(moduleName + ":GT: current_state=" + current_state.ToString(), textPanelReport); bWantFast = true; Echo("Going Target: state=" + current_state.ToString()); if (current_state == 0) { if ((craft_operation & CRAFT_MODE_SLED) > 0) { bSled = true; if (speedMax > 45) { speedMax = 45; } } else { bSled = false; } if ((craft_operation & CRAFT_MODE_ROTOR) > 0) { bRotor = true; if (speedMax > 15) { speedMax = 15; } } else { bRotor = false; } GyroControl.SetRefBlock(gpsCenter); if (bValidHome || bValidTarget) { current_state = 160; } else { setMode(MODE_ATTENTION); } } else if (current_state == 160) { // 160 move to Target Echo("Moving to Target"); Vector3D vTargetLocation = vHome; if (bValidTarget) { vTargetLocation = vTargetMine; } Vector3D vVec = vTargetLocation - gpsCenter.GetPosition(); double distance = vVec.Length(); Echo("distance=" + niceDoubleMeters(distance)); Echo("velocity=" + velocityShip.ToString("0.00")); // Echo("TL:" + vTargetLocation.X.ToString("0.00") + ":" + vTargetLocation.Y.ToString("0.00") + ":" + vTargetLocation.Z.ToString("0.00")); // if(distance<17) if (bGoOption && (distance < arrivalDistanceMin)) { Echo("we have arrived"); // bValidTargetLocation = false; gyrosOff(); ResetMotion(); bValidHome = false; // we used this one up. setMode(MODE_ARRIVEDTARGET); return; } bool bYawOnly = false; if (bSled || bRotor) { bYawOnly = true; } debugGPSOutput("TargetLocation", vTargetLocation); bool bAimed = false; double yawangle = -999; if (bYawOnly) { yawangle = CalculateYaw(vTargetLocation, gpsCenter); Echo("yawangle=" + yawangle.ToString()); bAimed = Math.Abs(yawangle) < .05; if (bSled) { DoRotate(yawangle, "Yaw"); } else if (bRotor) { DoRotorRotate(yawangle); } // else: WE DON"T KNOW WHAT WE ARE } else { bAimed = GyroMain("forward", vVec, gpsCenter); } //return; if (bAimed) { // we are aimed at location Echo("Aimed"); gyrosOff(); if (!bGoOption) { powerDownRotors(rotorNavLeftList); powerDownRotors(rotorNavRightList); powerDownThrusters(thrustAllList); setMode(MODE_ARRIVEDTARGET); return; } double stoppingDistance = calculateStoppingDistance(thrustBackwardList, velocityShip, 0); double dFar = 100; double dApproach = 50; double dPrecision = 15; if (velocityShip > 5) { dFar = stoppingDistance * 5; } if (velocityShip > 5) { dApproach = stoppingDistance * 2; } Echo("distance=" + niceDoubleMeters(distance)); // Echo("DFar=" + dFar); // Echo("dApproach=" + dApproach); // Echo("dPrecision=" + dPrecision); Echo("speedMax=" + speedMax); Echo("velocityShip=" + velocityShip); if (distance > dFar) { // Echo("DFAR"); if (velocityShip < 1) { Echo("DFAR*1"); powerForward(100); } else if (velocityShip < (speedMax * 0.85)) // else if (velocityShip < speedMax / 2) { Echo("DFAR**2"); powerForward(55); } else if (velocityShip < (speedMax * 1.05)) { Echo("DFAR***3"); powerForward(1); } else { Echo("DFAR****4"); powerDown(); } } else if (distance > dApproach) { Echo("Approach"); if (velocityShip < 1) { powerForward(100); } else if (velocityShip < speedMax / 2) { powerForward(25); } else if (velocityShip < speedMax) { powerForward(1); } else { powerDown(); } } else if (distance > dPrecision) { Echo("Precision"); // almost to target. should take stoppingdistance into account. if (velocityShip < 1) { powerForward(100); } else if (velocityShip < speedMax / 2) { powerForward(25); } else if (velocityShip < speedMax) { powerForward(1); } else { powerDown(); } } else { Echo("Close"); if (velocityShip < 1) { powerForward(25); } else if (velocityShip < 5) { powerForward(5); } else { powerDown(); } } } else { // we are aiming at location Echo("Aiming"); // DoRotate(yawangle, "Yaw"); // DO NOT turn off rotors.. powerDownThrusters(thrustAllList); } } }
public IEnumerator <bool> Init() { rc = GridTerminalSystem.GetBlockWithName(RC_Name) as IMyShipController; lcd = new Lcd(this); Echo("lcd loaded."); Echo("[|..........]"); yield return(true); sensor = new Sensor(rc, this); Echo("sensor loaded."); Echo("[||.........]"); yield return(true); wheel = new WheelControl(rc, this); Echo("wheel loaded."); Echo("[|||........]"); yield return(true); downforce = new Downforce(rc, this, DOWNFORCENAME); Echo("downforce loaded."); Echo("[||||.......]"); yield return(true); stopwatch = new Stopwatch(rc); Echo("stopwatch loaded."); Echo("[|||||......]"); yield return(true); logo = new Logo(this); Echo("logo loaded."); Echo("[||||||.....]"); yield return(true); transponder = new Transponder(this, PERSONSENSORNAME, ANTENNATAG); Echo("transponder loaded."); Echo("[|||||||....]"); yield return(true); lights = new Lights(rc, this, REARLIGHTGROUPNAME); Echo("lights loaded."); Echo("[||||||||...]"); yield return(true); gyroControl = new GyroControl(this, rc); Echo("gyrocontrol loaded."); Echo("[|||||||||..]"); yield return(true); IMyCameraBlock cam = GridTerminalSystem.GetBlockWithName(CAMERANAME) as IMyCameraBlock; surfaceScanner = new SurfaceScanner(cam, rc); Echo("surface scanner loaded"); Echo("[||||||||||.]"); yield return(true); dynamicCOM = new DynamicCOM(rc, this); Echo("DynamicCOM loaded"); Echo("[|||||||||||]"); Loaded = true; }
string doInit() { // initialization of each module goes here: // when all initialization is done, set init to true. if (currentInit == 0) { initLogging(); } Log("Init:" + currentInit.ToString()); double progress = currentInit * 100 / 3; string sProgress = progressBar(progress); StatusLog(moduleName + sProgress, textPanelReport); Echo("Init: " + currentInit.ToString()); if (currentInit == 0) { //StatusLog("clear",textLongStatus,true); StatusLog(DateTime.Now.ToString() + " " + OurName + ":" + moduleName + ":INIT", textLongStatus, true); if (!modeCommands.ContainsKey("findore")) { modeCommands.Add("findore", MODE_FINDORE); } gridsInit(); sInitResults += initSerializeCommon(); Deserialize(); initShipDim(); } else if (currentInit == 1) { sInitResults += BlockInit(); anchorPosition = gpsCenter; currentPosition = anchorPosition.GetPosition(); sInitResults += connectorsInit(); sInitResults += thrustersInit(gpsCenter); sInitResults += camerasensorsInit(gpsCenter); sInitResults += sensorInit(); // sInitResults += gearsInit(); sInitResults += tanksInit(); sInitResults += gyrosetup(); GyroControl.UpdateGyroList(gyros); sInitResults += drillInit(); sInitResults += ejectorsInit(); initCargoCheck(); Deserialize(); // bWantFast = false; sInitResults += modeOnInit(); init = true; } currentInit++; if (init) { currentInit = 0; } Log(sInitResults); Echo("Init exit"); return(sInitResults); }