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