public void Main(string argument, UpdateType updateSource) { GridTerminalSystem.GetBlocksOfType <IMyGyro>(gyros); GridTerminalSystem.GetBlocksOfType <IMyRemoteControl>(controllers); GridTerminalSystem.GetBlocksOfType <IMySensorBlock>(sensors); if (controllers.Count < 1 || gyros.Count < 1) { Echo("ERROR: either no controller or gyro"); return; } IMyRemoteControl controller = controllers[0];// //Vector2 yawAndRoll = controller.RotationIndicator; //Vector3 inputRotationVector = new Vector3(-yawAndRoll.X, yawAndRoll.Y, controller.RollIndicator);//pitch, yaw, roll. What?? ok. IMySensorBlock sensor = sensors[0]; //Echo($"{sensor.LastDetectedEntity.Position}\n{sensor.LastDetectedEntity.Orientation}\n{sensor.LastDetectedEntity.Velocity}"); //direction to the target Vector3D p1 = controller.GetPosition(); Vector3D p2 = sensor.LastDetectedEntity.Position; Vector3D p3 = p2 - p1; //rotation to the target p3.Normalize(); Vector3D heading = controller.WorldMatrix.Forward; heading.Normalize(); double angle = Math.Acos(heading.Dot(p3));//this doesn't account for up vector for now Echo($"ANGLE:{angle}"); Echo($"p3:{p3}"); antenna.HudText = angle.ToString(); //axis of rotation Vector3D cross = heading.Cross(p3); //var relativeRotation = Vector3D.Transform(cross, controller.WorldMatrix); double yaw; double pitch; GetRotationAngles(p3, controller.WorldMatrix.Forward, controller.WorldMatrix.Left, controller.WorldMatrix.Up, out yaw, out pitch); applyRotation(VECTOR@, gyros, controller); //turn axis into ship coordinates //calc the new matrix after that rotation //IF there is a gravity //calc angle to rotate by and axis of rotation, add that rotation to the rotation already underway //align the grav and up vector into the same plane...? //calc what my desired forward vector is(p3?), then //need to rotate and project up vector in line with art/nat gravity //Vector3D projection = a.Dot(b) / b.LengthSquared() * b; //applyRotation(inputRotationVector, gyros, controller); //keep this at the end previousShipVelicities = controllers[0].GetShipVelocities(); }
public void Update1() { MatrixD orientation = navigation.Orientation; MyShipVelocities shipVelocities = navigation.Velocities; UpdateGyroControl(orientation, shipVelocities.AngularVelocity); UpdateThrustControl(orientation, shipVelocities.LinearVelocity); }
public void Main(string argument, UpdateType updateSource)//main is called once per pysics update, ideally 60Hz { if (argument.ToLower() == "reset") { stage = 0; } bool valid = controller.TryGetPlanetElevation(MyPlanetElevation.Surface, out currentAltitude); var parts = Storage.Split(';'); stage = int.Parse(parts[0]); maxTotalEffectiveThrust = 0.0f;//have to recalculate every time because it increases with alt foreach (var thruster in thrusters) { maxTotalEffectiveThrust += thruster.MaxEffectiveThrust; } physicalMass = controller.CalculateShipMass().PhysicalMass; currentNaturalGravity = controller.GetNaturalGravity(); // double gravityMagnitude = currentNaturalGravity.Length(); //what a weird thing to call magnitude of a vector Echo($"STAGE: {stage}\nSPEED: {controller.GetShipVelocities().LinearVelocity.Length()}\nALT: {currentAltitude}"); antenna.HudText = stage.ToString(); if (stage == 0) { if (Freefall()) //if we are in freefall. Freefall is defined as accelerating at a rate ~= gravity, this implies thrusters are not acive. Also should check if it's heading towards or away from the gravity well. Linear alg time yay { foreach (var thruster in thrusters) { thruster.ThrustOverride = 0; } stage++; Storage = stage + ";"; } } else if (stage == 1) { if (!Freefall()) { stage = 0; } double a = maxTotalEffectiveThrust / physicalMass - currentNaturalGravity.Length(); //acceleration with full retro burn double b = -controller.GetShipVelocities().LinearVelocity.Length(); //velocity double c = currentAltitude; double thrustStopTime = -b / (2 * a); //y=ax^2+bx -> dy = 2ax+b | 0 = 2*a*x+b -> -b/2a=x Func <double, double> altitudeFunc = delegate(double x) { //x=time, y=altitude assuming full retro thrust return(a * x * x + b * x + c); }; double minHeight = altitudeFunc(thrustStopTime); //the lowest point of the trajectory IF thrust is engaged instantly double calculatedStoppingDistance = currentAltitude - minHeight; double safetyBuffer = Math.Abs(b) * 1.25 + craftHeight; //a coefficient in front of the safety buffer feels wrong double safetyStoppingDistance = calculatedStoppingDistance + safetyBuffer; if (!controller.DampenersOverride && currentAltitude <= safetyStoppingDistance) { stage++; Storage = stage + ";"; controller.DampenersOverride = true; } } else if (stage == 2)//descent burn has been initiated, goto stg 3 when a safe speed has been reached { antenna.HudText = stage.ToString(); if (controller.GetShipVelocities().LinearVelocity.Length() < safeVelocity) { stage++; Storage = stage + ";"; } } else if (stage == 3)//target safe descent speed has been reached, maintain low speed until touchdown to planet { float totalThrustNeededToHover = physicalMass * (float)currentNaturalGravity.Length(); float idealThrustPerThruster = (float)totalThrustNeededToHover / (float)numThrusters; float thrustRatio = thrusters[0].MaxThrust / thrusters[0].MaxEffectiveThrust;//actual output of thrust is similar to but different from the input set by code/user. float adjustedThrustPerThruster = idealThrustPerThruster * thrustRatio; foreach (var thruster in thrusters) { thruster.ThrustOverride = adjustedThrustPerThruster; } if (currentAltitude < touchdownHeight) { stage++; Storage = stage + ";"; foreach (var thruster in thrusters) { controller.DampenersOverride = false; thruster.ThrustOverride = 0.0f; } } } else if (stage == 4)//touchdown, turn stuff off pls { antenna.HudText = stage.ToString(); Echo($"TOUCHDOWN"); stage = 0; Storage = stage + ";"; Runtime.UpdateFrequency = UpdateFrequency.None;//stop running the script } lastVelocities = controller.GetShipVelocities(); }
void doModeGoTarget() { int iMode = thisProgram.wicoControl.IMode; int iState = thisProgram.wicoControl.IState; // StatusLog("clear", textPanelReport); // StatusLog(moduleName + ":Going Target!", textPanelReport); // StatusLog(moduleName + ":GT: iState=" + iState.ToString(), textPanelReport); // bWantFast = true; thisProgram.Echo("Going Target: state=" + iState.ToString()); if (NAVTargetName != "") { thisProgram.Echo(NAVTargetName); } string sNavDebug = ""; sNavDebug += "GT:S=" + iState; // sNavDebug += " MinE=" + NAVGravityMinElevation; // ResetMotion(); IMyShipController shipController = thisProgram.wicoBlockMaster.GetMainController(); Vector3D vNG = shipController.GetNaturalGravity(); double dGravity = vNG.Length(); if (iState == 0) { thisProgram.wicoTravelMovement.ResetTravelMovement(); // sStartupError+="\nStart movemenet: ArrivalMode="+NAVArrivalMode+" State="+NAVArrivalState; // if ((craft_operation & CRAFT_MODE_SLED) > 0) if (thisProgram.wicoWheels.HasSledWheels()) { bSled = true; if (shipSpeedMax > 45) { shipSpeedMax = 45; } } else { bSled = false; } // if ((craft_operation & CRAFT_MODE_ROTOR) > 0) if (thisProgram.wicoNavRotors.NavRotorCount() > 0) { bRotor = true; if (shipSpeedMax > 15) { shipSpeedMax = 15; } } else { bRotor = false; } // if ((craft_operation & CRAFT_MODE_WHEEL) > 0) if (thisProgram.wicoWheels.HasWheels()) { 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; shipController.TryGetPlanetElevation(MyPlanetElevation.Surface, out elevation); if (!bSled && !bRotor) { // if flying ship // make sure set to default if (thisProgram.wicoBlockMaster.DesiredMinTravelElevation < 0) { thisProgram.wicoBlockMaster.DesiredMinTravelElevation = 75; // for EFM getting to target 'arrived' radius } } if (bValidNavTarget) { if (elevation > thisProgram.wicoBlockMaster.HeightInMeters()) { iState = 150; } else { iState = 160; } } else { thisProgram.wicoControl.SetMode(WicoControl.MODE_ATTENTION); //else setMode(MODE_ATTENTION); } thisProgram.wicoControl.WantFast(); // bWantFast = true; } else if (iState == 150) { thisProgram.wicoControl.WantFast();// bWantFast = true; if (dGravity > 0) { double elevation = 0; shipController.TryGetPlanetElevation(MyPlanetElevation.Surface, out elevation); sNavDebug += " E=" + elevation.ToString("0.0"); float fSaveAngle = thisProgram.wicoGyros.GetMinAngle(); // minAngleRad; thisProgram.wicoGyros.SetMinAngle(0.1f); // minAngleRad = 0.1f; bool bAligned = GyroMain("", vNG, shipController); sNavDebug += " Aligned=" + bAligned.ToString(); thisProgram.Echo("bAligned=" + bAligned.ToString()); thisProgram.wicoGyros.SetMinAngle(fSaveAngle); //minAngleRad = fSaveAngle; if (bAligned || elevation < thisProgram.wicoBlockMaster.HeightInMeters() * 2) { thisProgram.wicoGyros.gyrosOff(); if (thisProgram.wicoBlockMaster.DesiredMinTravelElevation > 0) { iState = 155; } else { iState = 160; } } } else { iState = 160; } } else if (iState == 151) { thisProgram.wicoControl.WantFast();// bWantFast = true; if (dGravity > 0 || btmWheels) { double elevation = 0; shipController.TryGetPlanetElevation(MyPlanetElevation.Surface, out elevation); sNavDebug += " E=" + elevation.ToString("0.0"); float fSaveAngle = thisProgram.wicoGyros.GetMinAngle(); // minAngleRad; thisProgram.wicoGyros.SetMinAngle(0.1f); // minAngleRad = 0.1f; bool bAligned = GyroMain("", vNG, shipOrientationBlock); sNavDebug += " Aligned=" + bAligned.ToString(); thisProgram.Echo("bAligned=" + bAligned.ToString()); thisProgram.wicoGyros.SetMinAngle(fSaveAngle); //minAngleRad = fSaveAngle; if (bAligned || elevation < thisProgram.wicoBlockMaster.HeightInMeters() * 2) { thisProgram.wicoGyros.gyrosOff(); if (thisProgram.wicoBlockMaster.DesiredMinTravelElevation > 0) { iState = 155; } else { iState = 160; } } else { iState = 150; // try again to be aligned. } } else { iState = 160; } } else if (iState == 155) { // for use in gravity: aim at location using yaw only thisProgram.wicoControl.WantFast(); // bWantFast = true; if (bWheels) { iState = 160; return; } if (dGravity > 0) { bool bAligned = GyroMain("", vNG, shipController); sNavDebug += " Aligned=" + bAligned.ToString(); double yawangle = -999; yawangle = thisProgram.CalculateYaw(vNavTarget, shipController); bool bAimed = Math.Abs(yawangle) < 0.1; // NOTE: 2x allowance thisProgram.Echo("yawangle=" + yawangle.ToString()); sNavDebug += " Yaw=" + yawangle.ToString("0.00"); if (!bAimed) { if (btmRotor) { thisProgram.Echo("Rotor"); thisProgram.wicoNavRotors.DoRotorRotate(yawangle); } else // use for both sled and flight { thisProgram.wicoGyros.DoRotate(yawangle, "Yaw"); } } if (bAligned && bAimed) { thisProgram.wicoGyros.gyrosOff(); iState = 160; } else if (bAligned && Math.Abs(yawangle) < 0.5) { float atmo; float hydro; float ion; thisProgram.wicoThrusters.CalculateHoverThrust(shipController, thrustForwardList, out atmo, out hydro, out ion); atmo += 1; hydro += 1; ion += 1; thisProgram.wicoThrusters.powerUpThrusters(thrustForwardList, atmo, WicoThrusters.thrustatmo); thisProgram.wicoThrusters.powerUpThrusters(thrustForwardList, hydro, WicoThrusters.thrusthydro); thisProgram.wicoThrusters.powerUpThrusters(thrustForwardList, ion, WicoThrusters.thrustion); } else { thisProgram.wicoThrusters.powerDownThrusters(thrustForwardList); } } else { iState = 160; } } else if (iState == 156) { // realign gravity thisProgram.wicoControl.WantFast();// bWantFast = true; bool bAimed = GyroMain("", grav, shipOrientationBlock); if (bAimed) { thisProgram.wicoGyros.gyrosOff(); iState = 160; } } else if (iState == 160) { // 160 move to Target thisProgram.Echo("Moving to Target"); Vector3D vTargetLocation = vNavTarget; double velocityShip = shipController.GetShipSpeed(); Vector3D vVec = vTargetLocation - shipController.GetPosition(); double distance = vVec.Length(); thisProgram.Echo("distance=" + niceDoubleMeters(distance)); thisProgram.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)) { iState = 500; thisProgram.Echo("we have arrived"); thisProgram.wicoControl.WantFast();// bWantFast = true; return; } // debugGPSOutput("TargetLocation", vTargetLocation); bool bDoTravel = true; if (thisProgram.wicoBlockMaster.DesiredMinTravelElevation > 0 && dGravity > 0) { double elevation = 0; MyShipVelocities mysSV = shipController.GetShipVelocities(); Vector3D lv = mysSV.LinearVelocity; // ASSUMES: -up = gravity down Assuming ship orientation var upVec = shipController.WorldMatrix.Up; var vertVel = Vector3D.Dot(lv, upVec); // thisProgram.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 shipController.TryGetPlanetElevation(MyPlanetElevation.Surface, out elevation); sNavDebug += " E=" + elevation.ToString("0.0"); sNavDebug += " V=" + velocityShip.ToString("0.00"); thisProgram.Echo("Elevation=" + elevation.ToString("0.0")); thisProgram.Echo("MinEle=" + thisProgram.wicoBlockMaster.DesiredMinTravelElevation.ToString("0.0")); // double stopD = calculateStoppingDistance(thrustUpList, velocityShip, dGravity); double stopD = 0; if (vertVel < 0) { stopD = thisProgram.wicoThrusters.calculateStoppingDistance(thrustUpList, Math.Abs(vertVel), dGravity); } double maxStopD = thisProgram.wicoThrusters.calculateStoppingDistance(thrustUpList, thisProgram.wicoControl.fMaxWorldMps, dGravity); float atmo; float hydro; float ion; thisProgram.wicoThrusters.CalculateHoverThrust(thrustUpList, out atmo, out hydro, out ion); // sNavDebug += " SD=" + stopD.ToString("0"); if ( // !bSled && !bRotor && thisProgram.wicoBlockMaster.DesiredMinTravelElevation > 0) { if ( vertVel < -0.5 && // we are going downwards (elevation - stopD * 2) < thisProgram.wicoBlockMaster.DesiredMinTravelElevation) { // too low. go higher // Emergency thrust sNavDebug += " EM UP!"; bool bAligned = GyroMain("", grav, shipOrientationBlock); thisProgram.wicoThrusters.powerUpThrusters(thrustUpList, 100); bDoTravel = false; thisProgram.wicoControl.WantFast();// bWantFast = true; } else if (elevation < thisProgram.wicoBlockMaster.DesiredMinTravelElevation) { // 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); thisProgram.wicoThrusters.powerUpThrusters(thrustUpList, atmo, thrustatmo); thisProgram.wicoThrusters.powerUpThrusters(thrustUpList, hydro, thrusthydro); thisProgram.wicoThrusters.powerUpThrusters(thrustUpList, ion, thrustion); } else if (elevation > (maxStopD + thisProgram.wicoBlockMaster.DesiredMinTravelElevation * 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); thisProgram.wicoThrusters.powerDownThrusters(thrustUpList, thrustAll, true); bool bAligned = GyroMain("", grav, shipOrientationBlock); if (!bAligned) { thisProgram.wicoControl.WantFast();// bWantFast = true; bDoTravel = false; } // powerUpThrusters(thrustUpList, 1f); } else if ( elevation > thisProgram.wicoBlockMaster.DesiredMinTravelElevation * 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 += " ^"; thisProgram.wicoThrusters.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"); bool bAligned = GyroMain("", grav, shipOrientationBlock); if (!bAligned) { thisProgram.wicoControl.WantFast();// bWantFast = true; bDoTravel = false; } // bDoTravel = false; } } else { sNavDebug += " -"; atmo -= 5; hydro -= 5; ion -= 5; } thisProgram.wicoThrusters.powerUpThrusters(thrustUpList, atmo, thrustatmo); thisProgram.wicoThrusters.powerUpThrusters(thrustUpList, hydro, thrusthydro); thisProgram.wicoThrusters.powerUpThrusters(thrustUpList, ion, thrustion); } else { // normal hover thisProgram.wicoThrusters.powerDownThrusters(thrustUpList); } } } if (bDoTravel) { thisProgram.Echo("Do Travel"); thisProgram.wicoTravelMovement.doTravelMovement(vTargetLocation, (float)arrivalDistanceMin, 500, 300); } else { thisProgram.wicoThrusters.powerDownThrusters(thrustForwardList); } } else if (iState == 300) { // collision detection thisProgram.wicoControl.WantFast(); // bWantFast = true; Vector3D vTargetLocation = vNavTarget; thisProgram.wicoTravelMovement.ResetTravelMovement(); thisProgram.wicoTravelMovement.calcCollisionAvoid(vTargetLocation, lastDetectedInfo, out vAvoid); // iState = 301; // testing iState = 320; } else if (iState == 301) { // just hold this state thisProgram.wicoControl.WantFast();// bWantFast = true; } else if (iState == 320) { // Vector3D vVec = vAvoid - shipOrientationBlock.GetPosition(); // double distanceSQ = vVec.LengthSquared(); thisProgram.Echo("Primary Collision Avoid"); // StatusLog("clear", sledReport); // StatusLog("Collision Avoid", sledReport); // StatusLog("Collision Avoid", textPanelReport); thisProgram.wicoTravelMovement.doTravelMovement(vAvoid, 5.0f, 160, 340); } else if (iState == 340) { // secondary collision if ( lastDetectedInfo.Type == MyDetectedEntityType.LargeGrid || lastDetectedInfo.Type == MyDetectedEntityType.SmallGrid ) { iState = 345; } else if (lastDetectedInfo.Type == MyDetectedEntityType.Asteroid ) { iState = 350; } else { iState = 300; // setMode(MODE_ATTENTION); } thisProgram.wicoControl.WantFast(); // bWantFast = true; } else if (iState == 345) { // we hit a grid. align to it Vector3D[] corners = new Vector3D[BoundingBoxD.CornerCount]; BoundingBoxD bbd = lastDetectedInfo.BoundingBox; bbd.GetCorners(corners); GridUpVector = thisProgram.wicoSensors.PlanarNormal(corners[3], corners[4], corners[7]); GridRightVector = thisProgram.wicoSensors.PlanarNormal(corners[0], corners[1], corners[4]); thisProgram.wicoControl.WantFast();// bWantFast = true; iState = 348; } else if (iState == 348) { thisProgram.wicoControl.WantFast();// bWantFast = true; if (GyroMain("up", GridUpVector, shipOrientationBlock)) { iState = 349; } } else if (iState == 349) { thisProgram.wicoControl.WantFast();// bWantFast = true; if (GyroMain("right", GridRightVector, shipOrientationBlock)) { iState = 350; } } else if (iState == 350) { // initEscapeScan(bCollisionWasSensor, !bCollisionWasSensor); initEscapeScan(bCollisionWasSensor); thisProgram.wicoTravelMovement.ResetTravelMovement(); dtNavStartShip = DateTime.Now; iState = 360; thisProgram.wicoControl.WantFast();// bWantFast = true; } else if (iState == 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) { thisProgram.wicoControl.SetMode(WicoControl.MODE_ATTENTION);// setMode(MODE_ATTENTION); // doTriggerMain(); return; } if (scanEscape()) { thisProgram.Echo("ESCAPE!"); iState = 380; } thisProgram.wicoControl.WantMedium(); // bWantMedium = true; // bWantFast = true; } else if (iState == 380) { // StatusLog("Collision Avoid Travel", textPanelReport); thisProgram.Echo("Escape Collision Avoid"); thisProgram.wicoTravelMovement.doTravelMovement(vAvoid, 1f, 160, 340); } else if (iState == 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; thisProgram.wicoAntennas.SetMaxPower(false); thisProgram.wicoSensors.SensorsSleepAll(); // sStartupError += "Finish WP:" + wicoNavCommands.Count.ToString()+":"+NAVArrivalMode.ToString(); // set to desired mode and state thisProgram.wicoControl.SetMode(NAVArrivalMode);// setMode(NAVArrivalMode); iState = NAVArrivalState; // set up defaults for next run (in case they had been changed) NAVArrivalMode = WicoControl.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:")) { thisProgram.Echo("Found NAV: command:"); tList[i1].CustomName = "NAV: C Arrived Target"; } } } } thisProgram.wicoControl.WantFast();// bWantFast = true; // doTriggerMain(); } // NavDebug(sNavDebug); }
/// <summary> /// гаситель инерции /// </summary> private void InertionCompensator() { velocity = cockpit.GetShipVelocities(); // получение скорости корабля относительно мира Vector3D V3Dfow = cockpit.WorldMatrix.Forward; // вектор направления вперед у корабля Vector3D V3Dup = cockpit.WorldMatrix.Up; // вектор направления вверх у корабля Vector3D V3Dleft = cockpit.WorldMatrix.Left; // вектор направления влево у корабля Vector3D velocityV3Dleft = new Vector3D(velocity.LinearVelocity.X, 0, 0); // вектор скорости по оси X (лево/право) Vector3D velocityV3Dup = new Vector3D(0, velocity.LinearVelocity.Y, 0); // вектор скорости по оси Y (вверх/вниз) Vector3D velocityV3Dfow = new Vector3D(0, 0, velocity.LinearVelocity.Z); // вектор скорости по оси Z (вперед/назад) double projectionLeft = velocityV3Dleft.Length() * Vector3D.Dot(velocityV3Dleft, V3Dleft) + velocityV3Dfow.Length() * Vector3D.Dot(velocityV3Dfow, V3Dleft) + velocityV3Dup.Length() * Vector3D.Dot(velocityV3Dup, V3Dleft); double projectionUp = velocityV3Dleft.Length() * Vector3D.Dot(velocityV3Dleft, V3Dup) + velocityV3Dfow.Length() * Vector3D.Dot(velocityV3Dfow, V3Dup) + velocityV3Dup.Length() * Vector3D.Dot(velocityV3Dup, V3Dup); double projectionFow = velocityV3Dleft.Length() * Vector3D.Dot(velocityV3Dleft, V3Dfow) + velocityV3Dfow.Length() * Vector3D.Dot(velocityV3Dfow, V3Dfow) + velocityV3Dup.Length() * Vector3D.Dot(velocityV3Dup, V3Dfow); // нахождение длин проекций по 3 осям /* * Управление тягой вперед/назад */ if (projectionFow > 10) { ThrustOn(thrustBackward); ThrustOff(thrustForward); } else if (projectionFow < -10) { ThrustOn(thrustForward); ThrustOff(thrustBackward); } else { ThrustOff(thrustForward); ThrustOff(thrustBackward); } /* * Управление тягой влево/вправо */ if (projectionLeft > 10) { ThrustOn(thrustRight); ThrustOff(thrustLeft); } else if (projectionLeft < -10) { ThrustOn(thrustLeft); ThrustOff(thrustRight); } else { ThrustOff(thrustRight); ThrustOff(thrustLeft); } /* * Управление тягой вверх/вниз */ if (projectionUp > 10) { ThrustOn(thrustDown); ThrustOff(thrustUp); } else if (projectionUp < -10) { ThrustOn(thrustUp); ThrustOff(thrustDown); } else { ThrustOff(thrustUp); ThrustOff(thrustDown); } }
private void Update() { { //Apply handbrake if controller is not in use if (!controller.IsUnderControl && !controller.HandBrake) { ShipController.SetHandBrake(controller, true); } } MyShipVelocities vWorld = controller.GetShipVelocities(); Vector3D worldForward = GridToWorld(shipForward, controller.CubeGrid), vWorldForward = Project(vWorld.LinearVelocity, worldForward); float speedForward = (float)(vWorldForward.Length() * kmph_p_mps), vForward = speedForward * Math.Sign(Vector3D.Dot(vWorldForward, worldForward)); WheelControls controls = new WheelControls(controller); //Manage automatic gearbox if (status.gearsAutomatic) { //check which gear we should be in //-compare to vForward to limit to lowest gear for reversing if ((status.gear < gearMax) && (vForward >= 0.9f * gears[status.gear].speedLimitMax)) { status.gear++; status.modified = true; } else if ((status.gear > 0) && (vForward < 0.9f * gears[status.gear - 1].speedLimitMax)) { status.gear--; status.modified = true; } } //Pre-calculate traction control values float speedLimitMax = gears[status.gear].speedLimitMax, speedLimit = MyMath.Clamp(speedForward + 1.0f, 1.0f, speedLimitMax); float power = gears[status.gear].power; bool speedUnsafe = (speedForward >= speedLimitMax * 1.1f), brake = controls.brake || speedUnsafe; //Check each suspension unit //-only control suspension units on our grid //-check that we have access to the suspension unit GridTerminalSystem.GetBlocksOfType <IMyMotorSuspension>(temp); int count = 0; for (int i = 0; i < temp.Count; i++) { IMyMotorSuspension suspension = (IMyMotorSuspension)temp[i]; if (suspension.CubeGrid != controller.CubeGrid) { continue; } count++; //Check if we can actually use the suspension unit if (ValidateBlock(suspension, callbackRequired: false)) { //Apply traction control //-limit wheel speed based on vehicle's forward speed //-apply brakes while speed is unsafe MotorSuspension.SetSpeedLimit(suspension, speedLimit); if (!controller.HandBrake) { suspension.Brake = brake; } //Apply gear ratio if (suspension.Power != power) { MotorSuspension.SetPower(suspension, power); } } } //end for //Output status Echo("vF : " + speedForward.ToString("F1") + " kmph"); Echo("gear : " + gears[status.gear].name); Echo("mode : " + (status.gearsAutomatic ? "Auto" : "Manual")); //Echo(temp.Count.ToString() +" wheels found."); //Echo(count.ToString() +" processed."); }