private void PrintStatus()
{
PrintLine(" VECTOR MODULE ACTIVE");
PrintLine(" MODE: " + action?.name.ToUpper() + "\n");
string output = "";
if (action?.name == "brake")
{
var percent = Math.Abs(cockpit.GetShipSpeed() / startSpeed);
string progressBar;
progressBar = "|";
int width = 24;
var height = 3;
output = " PROGRESS\n";
for (var i = 0; i < width; i++)
{
progressBar += (i < width * (1 - percent)) ? "#" : " ";
}
progressBar += "|\n";
for (var i = 0; i < height; i++)
{
output += progressBar;
}
}
else
{
output = " Speed: " + Math.Abs(cockpit.GetShipSpeed()).ToString("000") + " m/s";
}
PrintLine(output);
}
void WheelsControl(float angle, int direction, float speed)
{
if (float.IsNaN(angle))
{
return;
}
SuspensionParametersChangedEventArgs args = new SuspensionParametersChangedEventArgs();
args.Speed = speed;
float KmH = (float)_controller.GetShipSpeed() * 3.7f;
_controller.HandBrake = KmH > args.Speed + 2.0f;// || _handBrake;
float speedFactor = KmH / _maxForwardSpeed;
args.Movement = -(1f - 0.7f * speedFactor) * direction;
args.Friction = 50.0f - 25.0f * speedFactor;
args.Steer = angle / MAX_STEER_ANGLE;
if (float.IsNaN(args.Steer))
{
args.Steer = 0;
}
ChangeTruckSuspensionParameters?.Invoke(this, args);
}
void SetDampenersOnline(bool dampOn)
{
if (shipControllers.Count <= 0)
{
return;
}
IMyShipController SHIP_CONTROLLER = null;
foreach (IMyShipController controler in shipControllers)
{
if (controler.IsWorking)
{
SHIP_CONTROLLER = controler; if (controler.IsMainCockpit)
{
break;
}
}
}
if (SHIP_CONTROLLER != null)
{
if (dampOn)
{
if (SHIP_CONTROLLER.GetShipSpeed() <= 0d)
{
SHIP_CONTROLLER.DampenersOverride = true;
}
}
else
{
SHIP_CONTROLLER.DampenersOverride = false;
}
}
}
private void SpeedControl()
{
SetSpeed(_currentSpeed + acceleration * (-controller.MoveIndicator.Z));
float currentSpeed = Math.Abs(_currentSpeed);
float movementDirection = Math.Sign(_currentSpeed);
float currentRightOverride = -movementDirection;
float currentLeftOverride = movementDirection;
if (controller.GetShipSpeed() * 3.7f > currentSpeed + 5.0f)
{
controller.HandBrake = true;
}
else
{
controller.HandBrake = handBrake;
}
foreach (IMyMotorSuspension wheel in wheels)
{
wheel.SetValueFloat("Speed Limit", currentSpeed);
if (wheel.CustomName.Contains("Right"))
{
wheel.SetValueFloat("Propulsion override", currentRightOverride);
}
else
{
wheel.SetValueFloat("Propulsion override", currentLeftOverride);
}
}
}
private IEnumerator <bool> TargetingRoutine()
{
if (control.GetNaturalGravity() == Vector3D.Zero && control.GetShipSpeed() < 2) // Easy
{
quartic = new QuarticTargeting(control.GetVelocityVector(), control.GetPosition(), maxProjectileVel);
Vector3D?result = quartic.CalculateTrajectory(target);
if (result.HasValue)
{
onRoutineFinish?.Invoke(result.Value);
}
else
{
onRoutineFail?.Invoke();
}
yield return(false);
}
else // This may take a while...
{
double timescale = 0.1;
var projectileInfo = new AdvancedSimTargeting.ProjectileInfo(200, maxProjectileVel, timescale, control.GetPosition(), control.GetVelocityVector());
simTargeting = new AdvancedSimTargeting(projectileInfo, target, control, ingameTime, 25, true, maxProjectileVel, timescale);
simTargeting.onSimComplete += OnSimValid;
simTargeting.onSimFail += OnSimFail;
yield return(true);
while (keepRunning)
{
simTargeting.Tick();
yield return(true);
}
}
}
public void Main()
{
if (accelerateTarget == null || accelerateTarget == Vector3D.Zero || accelerateTarget == Vector3D.PositiveInfinity || PID_Controller == null || !accelerateTarget.IsValid())
{
return;
}
Vector3D accelerateTargetNormalized = accelerateTarget;
double accelerateTargetLength = accelerateTargetNormalized.Normalize();
double error = Vector3D.Dot(control.WorldMatrix.Forward, accelerateTarget) + accelerateTargetLength;
double multiplier = Math.Abs(PID_Controller.NextValue(error, 0.016));
if (accelerateTargetLength < negligible)
{
accelerateTargetNormalized = Vector3D.Normalize(control.GetShipVelocities().LinearVelocity);
}
GyroUtils.PointInDirection(gyros, control, accelerateTargetNormalized, multiplier);
//ThrustUtils.SetThrustBasedDot(thrusters, accelerateTargetNormalized, multiplier);
autopilotModule.ThrustToVelocity(control.GetShipVelocities().LinearVelocity + accelerateTarget);
if (accelerateInDirection && control.GetShipSpeed() >= (speedLimit - 0.01))
{
accelerateTarget = Vector3D.Zero;
BoostForward(0);
accelerateInDirection = false;
}
}
private void WheelRotationSpeedLimiter()
{
if (!WheelRotationLimiter)
{
return;
}
float maxWheelSpeed = (float)rc.GetShipSpeed() + WheelRotationLimiterBuffer;
maxWheelSpeed *= 3.6f;
foreach (var wheel in wheels)
{
wheel.SetValueFloat("Speed Limit", maxWheelSpeed);
}
}
public void TravelToPosition(Vector3D position, bool enableAvoidance, double maximumVelocity = 100, double safetyMargin = 1.25, bool turnSelf = true)
{
double distanceFromTargetSQ = (position - ControlPosition).LengthSquared();
if (distanceFromTargetSQ > 1 || controller.GetShipSpeed() > 0.0001)
{
if (enableAvoidance)
{
if (avoidanceCheckCounter++ > 100)
{
double scanDistance = CalculateStoppingDistance() * safetyMargin;
avoidanceCheckCounter = 0;
collisionAvoidance.Scan(scanDistance);
}
if (collisionAvoidance.CheckForObjects())
{
Vector3D heading = Vector3D.Normalize(position - ControlPosition);
Vector3D nextDir = Vector3D.Zero;
collisionAvoidance.NextPosition(ref nextDir, heading);
ThrustToVelocity(nextDir * maximumVelocity);
gyroControl.StopRotation(gyros);
return;
}
}
TravelToPosition(position, maximumVelocity, safetyMargin, turnSelf);
}
}
private bool Travel()
{
double distToTarget = GetDistanceToTarget();
if (distToTarget < 2 * shipCenterToEdge)
{
return(true);
}
bool isEnd = false;
bool shouldBrake = CheckBrake();
if (shouldBrake)
{
ManageBrake();
if (timeSpentStationary > SHUTDOWN_TIME)
{
Debug("timeSpentStationary", timeSpentStationary);
isEnd = true;
}
}
else
{
float shipMass = referenceBlock.CalculateShipMass().PhysicalMass;
float speed = (float)referenceBlock.GetShipSpeed();
float secondsUntilVmax = 1;
float minLimitToZero = 1 / 100;
float maxLimitPercent = 98 / 100;
// F = m*a
// v_t = a_0*t + v_0 => a_0 = (v_t - v_0) / t
// F = m * (v_t - v_0) / t
float maxForceRequiered = shipMass * (maxInitialSpeed - speed) / secondsUntilVmax;
float percentForceRequiered = maxForceRequiered / GetAcceleratingForce();
if (percentForceRequiered > maxLimitPercent)
{
percentForceRequiered = maxLimitPercent;
}
if (percentForceRequiered < minLimitToZero)
{
percentForceRequiered = 0F;
}
GetThrusters(ThrustersFlag.Accelerating).ForEach(t => t.ThrustOverridePercentage = percentForceRequiered);
}
return(isEnd);
}
public void Seperation()
{
Echo("Waiting for RC to reach target");
velo = RController.GetShipSpeed();
if (!AutoEnable && (velo < 0.5))
{
Main("Return");
}
return;
}
IEnumerable <int> AccelerateCruise(int selfInstance)
{
yield return(1);
engineMgr.tsPower(ThrustFlags.Back, 100);
while (selfInstance == cruiseInstance && shipCtrl.GetShipSpeed() < maxTopSpeed)
{
yield return(20);
}
engineMgr.tsPower(ThrustFlags.Back, 0);
}
void UpdateOrientationParameters()
{
Vector3D linearVelocity = Vector3D.Normalize(cockpit_.GetShipVelocities().LinearVelocity);
Vector3D gravity = -Vector3D.Normalize(cockpit_.GetNaturalGravity());
pitch_ = Math.Acos(Vector3D.Dot(cockpit_.WorldMatrix.Forward, gravity)) * RadToDeg;
roll_ = Math.Acos(Vector3D.Dot(cockpit_.WorldMatrix.Right, gravity)) * RadToDeg;
worldSpeedForward_ = Vector3D.Dot(linearVelocity, Vector3D.Cross(gravity, cockpit_.WorldMatrix.Right)) * cockpit_.GetShipSpeed();
worldSpeedRight_ = Vector3D.Dot(linearVelocity, Vector3D.Cross(gravity, cockpit_.WorldMatrix.Forward)) * cockpit_.GetShipSpeed();
worldSpeedUp_ = Vector3D.Dot(linearVelocity, gravity) * cockpit_.GetShipSpeed();
}
public void Main()
{
if (started)
{
ticks++;
speed += rc.GetShipSpeed();
distanceTravelled += (long)Math.Round(Vector3D.Distance(prevLoc, rc.GetPosition()));
prevLoc = rc.GetPosition();
}
}
public bool Update()
{
if (Controller != null)
{
newValue = Controller.GetShipSpeed();
if (newValue != Value)
{
Value = newValue;
return(true);
}
}
return(false);
}
void GetController()
{
if (!(Controller ?? (Controller = Controllers[0])).IsUnderControl || !Controller.CanControlShip)
{
for (int i = 1; i < Controllers.Count; i++)
{
if (Controllers[i].IsUnderControl && Controllers[i].CanControlShip)
{
Controller = Controllers[i];
break;
}
}
}
if (Controller.GetShipSpeed() >= 12 && Controller.RollIndicator == 0)
{
if (Controller.GetShipSpeed() >= 40)
{
whangle = .17f;
}
else
{
whangle = .26f;
}
}
else
{
whangle = .38f;
}
if (Tangle != whangle)
{
Tangle = whangle;
foreach (IMyMotorSuspension w in Wheels)
{
w.MaxSteerAngle = Tangle;
}
}
}
private void doorCheck()
{
if (controller.GetShipSpeed() > ABORT_SPEED)
{
Echo("Autodoor disabled: ship is moving");
return;
}
// For each door, calculate appropriate bounding box where player must stand
foreach (var d in doors)
{
var bb = d.WorldAABB;
var forwardDir = bb.Matrix.Forward;
var backDir = bb.Matrix.Backward;
var ptForward = d.GetPosition() + doorSensorDistanceFrontRear * forwardDir;
var ptBackward = d.GetPosition() + doorSensorDistanceFrontRear * backDir;
var expandedBB = bb.Include(ptForward).Include(ptBackward);
bool openDoor = false;
// Check each door against all sensors
foreach (var sensor in sensors)
{
var objects = new List <MyDetectedEntityInfo>();
sensor.DetectedEntities(objects);
// Process each detected object, check against door list and make sure object is a human
foreach (var obj in objects)
{
if (obj.Type == MyDetectedEntityType.CharacterHuman && obj.Relationship != MyRelationsBetweenPlayerAndBlock.Enemies)
{
if (expandedBB.Contains(obj.Position) == ContainmentType.Contains)
{
openDoor = true;
}
}
}
}
if (openDoor)
{
d.ApplyAction("Open_On");
}
else
{
d.ApplyAction("Open_Off");
}
}
}
public VectorModule(CustomDataConfig config, GyroController gyroController, IMyShipController cockpit)
{
this.gyroController = gyroController;
this.cockpit = cockpit;
thrustVector = GetThrustVector(config.Get <string>("spaceMainThrust"));
AddAction("disabled", (args) => { gyroController.SetGyroOverride(false); }, null);
AddAction("brake", (args) => {
startSpeed = cockpit.GetShipSpeed();
cockpit.DampenersOverride = false;
}, SpaceBrake);
AddAction("prograde", null, () => { TargetOrientation(-Vector3D.Normalize(cockpit.GetShipVelocities().LinearVelocity)); });
AddAction("retrograde", null, () => { TargetOrientation(Vector3D.Normalize(cockpit.GetShipVelocities().LinearVelocity)); });
}
public Vector2 CalculatePitchRollToAchiveVelocity(Vector3 targetVelocity)
{
Vector3 diffrence = Vector3.Normalize(controller.GetShipVelocities().LinearVelocity - targetVelocity);
Vector3 gravity = -Vector3.Normalize(controller.GetNaturalGravity());
float velocity = (float)controller.GetShipSpeed();
float proportionalModifier = (float)Math.Pow(Math.Abs(diffrence.Length()), 2);
float pitch = NotNaN(Vector3.Dot(diffrence, Vector3.Cross(gravity, controller.WorldMatrix.Right)) * velocity) * proportionalModifier / dampeningFactor;
float roll = NotNaN(Vector3.Dot(diffrence, Vector3.Cross(gravity, controller.WorldMatrix.Forward)) * velocity) * proportionalModifier / dampeningFactor;
pitch = MinAbs(pitch, 90.0f * degToRad);
roll = MinAbs(roll, 90.0f * degToRad);
return(new Vector2(roll, pitch));
}
void UpdateTelemetry()
{
prevSpeed = currSpeed;
currAccl = (currSpeed = sc.GetShipSpeed()) - prevSpeed;
gravity = (gravityStrength = sc.GetNaturalGravity().Length()) / 9.81;
if (shipMassUpdateTick < Pgm.totalTicks)
{
shipMassUpdateTick = Pgm.totalTicks + TPS;
shipMass = sc.CalculateShipMass();
}
shipWeight = gravityStrength * shipMass.PhysicalMass; // or -> shipMass.TotalMass
prevAltitude = altitudeSurface;
if (sc.TryGetPlanetElevation(MyPlanetElevation.Surface, out altitudeSurface))
{
altitudeDiff = altitudeSurface - prevAltitude;
}
else
{
altitudeSurface = altitudeDiff = double.NaN;
}
atmosphereDensity = parachute?.Atmosphere ?? float.NaN;
if (null != downCamera)
{
if (downCamera.CanScan(1000))
{
MyDetectedEntityInfo dei = downCamera.Raycast(1000);
double len = 0;
if (null != dei.HitPosition)
{
Vector3D hp = (Vector3D)dei.HitPosition;
len = (hp - downCamera.CubeGrid.GetPosition()).Length();
}
raycastName = dei.Name + "/" + len; //downCamera.CubeGrid.GetPosition(); //dei.HitPosition.ToString();
}
else
{
raycastName = downCamera.AvailableScanRange.ToString();
}
}
}
public void OnEntityDetected(HaE_Entity entity)
{
if (Vector3D.DistanceSquared(entity.entityInfo.Position, control.GetPosition()) > 1000 * 1000)
{
return;
}
if (IgnoreEventAfterOnce)
{
return;
}
if (control.GetShipSpeed() < maxActiveRotorGunVel)
{
gridCannonTargeting.NewTarget(entity.entityInfo);
}
else
{
foreach (var cannon in rotorTurretGroups)
{
cannon.TargetDirection(ref Vector3D.Zero);
}
}
statusWriter.UpdateStatus(StatusWriter.TargetingStatus.Targeting);
missileCannonTargeting.NewTarget(entity.entityInfo);
basicTargeting.UpdateValues(control.GetVelocityVector(), control.GetPosition(), maxGatlingBulletVel);
var result = basicTargeting.CalculateTrajectory(entity.entityInfo);
if (result.HasValue)
{
TargetSolvedGatling(result.Value);
}
IgnoreEventAfterOnce = true;
}
private void CalculateStopDistance(IMyShipController controller, List <IMyThrust> thrusters)
{
float mass = controller.CalculateShipMass().PhysicalMass;
double stopThrustAvailable = 0;
int disabledThrusters = 0;
thrusters.ForEach(thruster =>
{
if (!thruster.IsWorking)
{
disabledThrusters++;
}
if (thruster.IsFunctional)
{
stopThrustAvailable += thruster.MaxEffectiveThrust;
}
});
StopThrustersWarning = disabledThrusters > 0;
double deacceleration = -stopThrustAvailable / mass;
double currentSpeed = controller.GetShipSpeed();
StoppingTime = (float)(-currentSpeed / deacceleration);
StoppingDistance = (float)(currentSpeed * StoppingTime + (deacceleration * StoppingTime * StoppingTime) / 2.0f);
}
public void Main(string argument, UpdateType updateType)
{
time += Runtime.TimeSinceLastRun;
if (!Initialized && time.Seconds % 5 == 0)
{
//Run Init every 5 seconds if not initialized.
Initialized = Init();
}
if (Initialized)
{
if ((updateType & (UpdateType.Terminal | UpdateType.Trigger)) != 0)
{
Input(argument);
}
if ((updateType & UpdateType.Update10) != 0)
{
//screen.WriteText($"Align: {(Align != null && Align.Enabled ? "on" : "off")}, Cruise: {(Cruise != null && Cruise.Enabled ? "on" : "off")}");
if (Align != null && Align.Enabled)
{
Align.Update();
}
if (Cruise != null && Cruise.Enabled)
{
Cruise.Update(Runtime.TimeSinceLastRun.TotalSeconds);
}
if (LCDs != null)
{
ScreenData data = new ScreenData();
data.maxAlt = targetAltAscending;
data.minAlt = targetAltDescending;
data.maxSpeed = worldTopSpeed;
if (Cruise != null)
{
if (Cruise.Enabled)
{
data.speed = Cruise.Speed;
data.targetSpeed = Cruise.AdaptiveTargetSpeed;
}
else if (MainController != null)
{
data.speed = MainController.GetShipSpeed();
data.targetSpeed = targetSpeed;
}
data.thrustOverrideH2 = Cruise.ThrustOverrideHydrogen;
data.thrustOverridePW = Cruise.ThrustOverrideElectric;
data.cruiseEnabled = Cruise.Enabled;
}
else
{
if (MainController != null)
{
data.speed = MainController.GetShipSpeed();
}
data.targetSpeed = targetSpeed;
}
if (Align != null)
{
data.alignEnabled = Align.Enabled;
}
LCDs.Update(data);
}
}
}
DetailedInfo();
BroadcastStatus();
//DrawScreens();
}
void StabilizePod()
{
//---Get speed
currentSpeed = referenceBlock.GetShipSpeed();
//---Dir'n vectors of the reference block
var referenceMatrix = referenceBlock.WorldMatrix;
var referenceForward = referenceMatrix.Forward;
var referenceLeft = referenceMatrix.Left;
var referenceUp = referenceMatrix.Up;
var referenceOrigin = referenceMatrix.Translation;
//---Get gravity vector
gravityVec = referenceBlock.GetNaturalGravity();
gravityVecMagnitude = gravityVec.Length();
if (gravityVec.LengthSquared() == 0)
{
foreach (IMyGyro thisGyro in gyros)
{
thisGyro.GyroOverride = false;
}
shouldStabilize = false;
angleRoll = 0;
angleRoll = 0;
downSpeed = 0;
return;
}
shipVelocityVec = referenceBlock.GetShipVelocities().LinearVelocity;
if (shipVelocityVec.LengthSquared() > maxSpeed * maxSpeed)
{
maxSpeed = shipVelocityVec.Length();
}
downSpeed = VectorProjection(shipVelocityVec, gravityVec).Length() * Math.Sign(shipVelocityVec.Dot(gravityVec));
//---Determine if we should manually override brake controls
altitude = 0;
referenceBlock.TryGetPlanetElevation(MyPlanetElevation.Surface, out altitude);
altitude -= shipHeight; //adjusts for height of the ship
brakeAltitudeThreshold = GetBrakingAltitudeThreshold();
stabilizeAltitudeThreshold = brakeAltitudeThreshold + 10 * currentSpeed; //this gives us a good safety cushion for stabilization procedures
//Echo($"Braking distance: {Math.Round(brakeAltitudeThreshold).ToString()}");
if (altitude < 100 && currentSpeed < 1)
{
timeSpentStationary += timeCurrentCycle;
}
else
{
timeSpentStationary = 0;
if (altitude <= stabilizeAltitudeThreshold)
{
shouldStabilize = true;
}
else
{
shouldStabilize = false;
}
if (altitude <= brakeAltitudeThreshold)
{
shouldBrake = true;
//kills dampeners to stop their interference with landing procedures
referenceBlock.DampenersOverride = false;
}
else
{
shouldBrake = false;
}
}
if (shouldBrake)
{
if (downSpeed > descentSpeed)
{
BrakingOn();
}
else
{
if (attemptToLand)
{
BrakingThrust();
}
else
{
ShutownSystems();
}
}
}
else
{
BrakingOff();
}
Vector3D alignmentVec = new Vector3D(0, 0, 0);
//--Check if drift compensation is on
if (useDriftCompensation && downSpeed > descentSpeed)
{
alignmentVec = shipVelocityVec;
}
else
{
alignmentVec = gravityVec;
}
//---Get Roll and Pitch Angles
anglePitch = Math.Acos(MathHelper.Clamp(alignmentVec.Dot(referenceForward) / alignmentVec.Length(), -1, 1)) - Math.PI / 2;
///////////////
///
Vector3D planetRelativeLeftVec = referenceForward.Cross(alignmentVec); //w.H.i.p.L.A.s.h.1.4.1
angleRoll = Math.Acos(MathHelper.Clamp(referenceLeft.Dot(planetRelativeLeftVec) / planetRelativeLeftVec.Length(), -1, 1));
angleRoll *= Math.Sign(VectorProjection(referenceLeft, alignmentVec).Dot(alignmentVec)); //ccw is positive
anglePitch *= -1;
angleRoll *= -1;
roll_deg = Math.Round(angleRoll / Math.PI * 180);
pitch_deg = Math.Round(anglePitch / Math.PI * 180);
//---Angle controller
double rollSpeed = Math.Round(angleRoll, 2);
double pitchSpeed = Math.Round(anglePitch, 2);
//---Enforce rotation speed limit
if (Math.Abs(rollSpeed) + Math.Abs(pitchSpeed) > 2 * Math.PI)
{
double scale = 2 * Math.PI / (Math.Abs(rollSpeed) + Math.Abs(pitchSpeed));
rollSpeed *= scale;
pitchSpeed *= scale;
}
if (shouldStabilize)
{
ApplyGyroOverride(pitchSpeed, 0, -rollSpeed, gyros, referenceBlock);
}
else
{
foreach (IMyGyro thisGyro in gyros)
{
thisGyro.GyroOverride = false;
}
}
}
public void Main(string input)
{
lcds = SearchBlocksWithName <IMyTextPanel>(lcdSearchName);
ClearOutput();
if (input == "start")
{
Runtime.UpdateFrequency = UpdateFrequency.Update10;
Autopilot = true;
}
IMyShipController controlBlock = (IMyShipController)GridTerminalSystem.GetBlockWithName(ShipControllerName);
double altitude = 0;
bool InsideNaturalGravity = controlBlock.TryGetPlanetElevation(MyPlanetElevation.Surface, out altitude);
Vector3D velocity3D = controlBlock.GetShipVelocities().LinearVelocity;
if (!InsideNaturalGravity)
{
if (Autopilot)
{
WriteLine("Waiting for entering natural gravity");
if (input == "stop")
{
Autopilot = false;
WriteLine("Autopilot deactivated (manually)");
}
}
return;
}
else
{
if (Autopilot && AutoFall)
{
if (!AutoFallUsed)
{
input = "fall";
AutoFallUsed = true;
}
}
}
List <IMyThrust> thrusters = GetBlocksInGroup <IMyThrust>(HydrogenThrustersGroupName);
ThrustController thrustController = new ThrustController(thrusters);
gyros = GetBlocksOfType <IMyGyro>();
gyroController = new GyroController(controlBlock, gyros, Base6Directions.Direction.Down, RotationSpeedLimit);
Vector3D gravity = controlBlock.GetNaturalGravity();
Vector3D position = controlBlock.GetPosition(); // ship coords
double gravityStrength = gravity.Length(); // gravity in m/s^2
double totalMass = controlBlock.CalculateShipMass().TotalMass; // ship total mass including cargo mass
double baseMass = controlBlock.CalculateShipMass().BaseMass; // mass of the ship without cargo
double cargoMass = totalMass - baseMass; // mass of the cargo
double actualMass = baseMass + (cargoMass / InventoryMultiplier); // the mass the game uses for physics calculation
double shipWeight = actualMass * gravityStrength; // weight in newtons of the ship
double velocity = controlBlock.GetShipSpeed(); // ship velocity
double brakeDistance = CalculateBrakeDistance(gravityStrength, actualMass, altitude, thrustController.availableThrust, velocity);
double brakeAltitude = StopAltitude + brakeDistance; // at this altitude the ship will start slowing Down
if (Autopilot)
{
gyroController.Align(gravity);
if (input == "fall")
{
// This is a workaround to a game bug (ship speed greater than speed limit when free falling in natural gravity)
// Pros: your ship will not crash. Cons: you will waste a tiny amount of hydrogen.
thrustController.ApplyThrust(1);
}
if (altitude <= (brakeAltitude + AltitudeMargin))
{
// BRAKE!!!
thrustController.ApplyFullThrust(); // Maybe just enable dampeners
}
if (altitude <= (StopAltitude + DisableMargin + AltitudeMargin))
{
if (velocity < StopSpeed)
{
gyroController.Stop();
WriteLine("Autopilot deactivated (automatically)");
}
if (SmartDeactivation)
{
if (OldVelocity3D.X * velocity3D.X < 0 || OldVelocity3D.Y * velocity3D.Y < 0 || OldVelocity3D.Z * velocity3D.Z < 0)
{
gyroController.Stop();
WriteLine("Autopilot deactivated (automatically)");
}
}
}
}
OldVelocity3D = velocity3D;
if (input == "stop")
{
Runtime.UpdateFrequency = UpdateFrequency.None;
gyroController.Stop();
thrustController.Stop();
WriteLine("Autopilot deactivated (manually)");
}
}
public void Main(string arg)
{
RController = GridTerminalSystem.GetBlockWithName(RC) as IMyShipController;
if (RController == null)
{
Echo(RCFailedMSG);
RCFailed = true;
Status = "Failed";
return;
}
RControllers = GridTerminalSystem.GetBlockWithName(RC) as IMyRemoteControl;
if (RControllers == null)
{
Echo(RCFailedMSG);
RCFailed = true;
Status = "Failed";
return;
}
var CCruise = GridTerminalSystem.GetBlockWithName(CC) as IMyProgrammableBlock;
if (CCruise == null)
{
Echo(CCFailedMSG);
CCFailed = true;
Status = "Failed";
return;
}
RGyro = GridTerminalSystem.GetBlockWithName(Gyro) as IMyGyro;
if (RGyro == null)
{
Echo(GyroFailedMSG);
GyroFailed = true;
Status = "Failed";
return;
}
RGyros = GridTerminalSystem.GetBlockWithName(Gyro) as IMyFunctionalBlock;
if (RGyros == null)
{
Echo(GyroFailedMSG);
GyroFailed = true;
Status = "Failed";
return;
}
RCon = GridTerminalSystem.GetBlockWithName(Cargo) as IMyCargoContainer;
if (RCon == null)
{
Echo(RConFailedMSG);
RConFailed = true;
Status = "Failed";
return;
}
LAntenna = GridTerminalSystem.GetBlockWithName(LA) as IMyLaserAntenna;
if (LAntenna == null)
{
Echo(LAFailedMSG);
LAFailed = true;
Status = "Failed";
return;
}
LGear = GridTerminalSystem.GetBlockWithName(LG) as IMyTimerBlock;
if (LGear == null)
{
Echo(LGFailedMSG);
LGFailed = true;
Status = "Failed";
return;
}
CCUp = GridTerminalSystem.GetBlockWithName(CCU) as IMyTimerBlock;
if (CCUp == null)
{
Echo(CCTsFailedMSG);
CCTsFailed = true;
Status = "Failed";
return;
}
CCOff = GridTerminalSystem.GetBlockWithName(CCO) as IMyTimerBlock;
if (CCOff == null)
{
Echo(CCTsFailedMSG);
CCTsFailed = true;
Status = "Failed";
return;
}
CCDown = GridTerminalSystem.GetBlockWithName(CCD) as IMyTimerBlock;
if (CCDown == null)
{
Echo(CCTsFailedMSG);
CCTsFailed = true;
Status = "Failed";
return;
}
GridTerminalSystem.GetBlocksOfType(thrustersUP, x => x.WorldMatrix.Forward == RControllers.WorldMatrix.Up);
if (thrustersUP.Count == 0)
{
Echo($"Error: No lift-off thrusters were found!");
}
GridTerminalSystem.GetBlocksOfType(thrustersDOWN, x => x.WorldMatrix.Forward == RControllers.WorldMatrix.Down);
if (thrustersDOWN.Count == 0)
{
Echo($"Error: No lift-off thrusters were found!");
}
var shipMass = RController.CalculateShipMass();
TotalMass = shipMass.TotalMass;
AutoEnable = RControllers.IsAutoPilotEnabled;
RController.TryGetPlanetElevation(MyPlanetElevation.Sealevel, out Elev);
velo = RController.GetShipSpeed();
Position = RController.GetPosition();
Gravity = (RController.GetNaturalGravity().Length());
GravityG = (RController.GetNaturalGravity().Length() / 9.81);
thrustSumUP = 0;
thrustSumDOWN = 0;
foreach (var block in thrustersUP)
{
thrustSumUP += block.MaxEffectiveThrust;
}
foreach (var block in thrustersDOWN)
{
thrustSumDOWN += block.MaxEffectiveThrust;
}
if (Status == "Launched")
{
TargetGravity = (PlanetGravity * (Math.Pow((MaxR / (TargetAltitude + MinR)), 7)));
Accel = ((thrustSumUP / TotalMass) + TargetGravity); //Stopping force
AccelTime = ((0 - velo) / -Accel); //Seconds to stop
stopDistance = ((velo * AccelTime) + ((-Accel * (AccelTime * AccelTime)) / 2));
}
if (Status == "Return")
{
Accel = ((thrustSumDOWN / TotalMass) - PlanetGravity); //Stopping force
AccelTime = ((0 - velo) / -Accel); //Seconds to stop
stopDistance = ((velo * AccelTime) + ((-Accel * (AccelTime * AccelTime)) / 2));
}
Echo(Ship + " Control Pro");
Echo("Status: " + Status);
Echo("Altitude: " + Math.Round(Elev, 2) + "/" + TargetAltitude);
Echo("Speed: " + Math.Round(velo, 2));
Echo("Total Weight: " + TotalMass);
Echo("Gravity: " + Math.Round(GravityG, 2) + "G");
string msg = ("Ship" + ":" + Ship + "," + "Status" + ":" + Status + "," + "Elevation" + ":" + Elev + "," + "Position" + ":" + Position + "," + "Speed" + ":" + velo + "," + "Target" + ":" + TargetAltitude + ",");
LAntenna.TransmitMessage(msg);
var keyValuePairs = arg.Split(',').Select(x => x.Split(':')).Where(x => x.Length == 2).ToDictionary(x => x.First(), x => x.Last());
//Echo(thrustSumDOWN.ToString());
//Echo(thrustersDOWN.Count.ToString());
//Echo(thrustSumUP.ToString());
//Echo(thrustersUP.Count.ToString());
if (Status == "Failed")
{
if (RCFailed == true)
{
Echo(RCFailedMSG); return;
}
if (CCFailed == true)
{
Echo(CCFailedMSG); return;
}
if (GyroFailed == true)
{
Echo(GyroFailedMSG); return;
}
if (LAFailed == true)
{
Echo(LAFailedMSG); return;
}
if (LGFailed == true)
{
Echo(LGFailedMSG); return;
}
if (RConFailed == true)
{
Echo(RConFailedMSG); return;
}
if (CCTsFailed == true)
{
Echo(CCTsFailedMSG); return;
}
Status = "Failed";
return;
}
if (arg.Contains("Target"))
{
TargetAltitudeSetter = keyValuePairs["Target"];
TargetAltitude = int.Parse(TargetAltitudeSetter);
NotReady();
}
if (!Init)
{
Status = "Initalizing..."; NotReady();
}
if (arg == "Reset")
{
Status = "Not Ready"; NotReady();
}
if (arg == "Ready")
{
Status = "Ready"; Ready();
}
if (arg == "Launch")
{
Status = "Launching"; Launch();
}
if (arg == "Launched")
{
Status = "Launched"; Climb();
}
if (arg == "Seperate")
{
Status = "Seperation"; Seperation();
}
if (arg == "Return")
{
Status = "Return"; Return();
}
if (arg == "Approach")
{
Status = "Approaching"; Approach();
}
if (arg == "Land")
{
Status = "Landing"; Land();
}
if (arg == "Landed")
{
Status = "Landed";
}
if (Status == "Launching")
{
Launch();
}
if (Status == "Launched")
{
Climb();
}
if (Status == "Seperation")
{
Seperation();
}
if (Status == "Return")
{
Return();
}
if (Status == "Approaching")
{
Approach();
}
if (Status == "Landing")
{
Land();
}
if (Status == "Landed")
{
Status = "Landed";
}
}
private void StabilizePod()
{
//---Get speed
double currentSpeed = referenceBlock.GetShipSpeed();
Vector3D vectDistanceToEnd = destination.ToVector3D() - referenceBlock.GetPosition();
Vector3D shipVelocityVec = referenceBlock.GetShipVelocities().LinearVelocity;
double brakingSpeed = VectorUtils.Projection(shipVelocityVec, vectDistanceToEnd).Length() * Math.Sign(shipVelocityVec.Dot(vectDistanceToEnd));
//---Determine if we should manually override brake controls
double distanceToEnd = vectDistanceToEnd.Length();
//--- Manage gravity
Vector3D gravityVec = referenceBlock.GetNaturalGravity();
double gravityVecMagnitude = gravityVec.Length();
if (gravityVecMagnitude > 0.0001)
{
double distanceToSurface;
referenceBlock.TryGetPlanetElevation(MyPlanetElevation.Surface, out distanceToSurface);
// TODO take into account this distance if we fly near the surface
}
distanceToEnd -= shipCenterToEdge;
double distanceToStartBraking = GetBrakingDistanceThreshold(gravityVecMagnitude, shipVelocityVec);
//this gives us a good safety cushion for stabilization procedures
double distanceToStartStabilize = distanceToStartBraking + 10 * currentSpeed;
bool shouldBrake = false;
bool shouldStabilize = false;
if (distanceToEnd < 100 && currentSpeed < 1)
{
timeSpentStationary += timeCurrentCycle;
}
else
{
timeSpentStationary = 0;
shouldStabilize = distanceToEnd <= distanceToStartStabilize;
shouldBrake = distanceToEnd <= distanceToStartBraking;
//kills dampeners to stop their interference with landing procedures
if (shouldBrake)
{
referenceBlock.DampenersOverride = false;
}
}
if (shouldBrake)
{
bool isBraking = brakingSpeed > FROM_BRAKING_TO_END_SPEED;
if (isBraking)
{
BrakingOn();
}
else
{
if (attemptToLand)
{
BrakingThrust(brakingSpeed, gravityVecMagnitude);
}
else
{
StopSystem(); // Fin
}
}
}
else
{
BrakingOff();
}
if (shouldStabilize)
{
//---Get Roll and Pitch Angles
Vector3D alignmentVec = brakingSpeed > FROM_BRAKING_TO_END_SPEED || gravityVecMagnitude <= 0 ? shipVelocityVec : gravityVec;
//---Dir'n vectors of the reference block
var referenceMatrix = referenceBlock.WorldMatrix;
var referenceForward = referenceMatrix.Forward;
var referenceLeft = referenceMatrix.Left;
var referenceUp = referenceMatrix.Up;
var referenceOrigin = referenceMatrix.Translation;
// Roll = rotation around forward/backward axis => to roll
// Pitch = rotation around left/right axis => to go up or down
// Yaw = rotation around up/down axis => to go left or right
double forAngleRadPitch = MathHelper.Clamp(alignmentVec.Dot(referenceForward) / alignmentVec.Length(), -1, 1);
double anglePitch = Math.Acos(forAngleRadPitch); // TODO test mais on freine vers l'avant pas le bas : - Math.PI / 2;
// ???
Vector3D relativeLeftVec = referenceForward.Cross(alignmentVec);
double forAngleRadRoll = MathHelper.Clamp(referenceLeft.Dot(relativeLeftVec) / relativeLeftVec.Length(), -1, 1);
double angleRoll = Math.Acos(forAngleRadRoll);
angleRoll *= Math.Sign(VectorUtils.Projection(referenceLeft, alignmentVec).Dot(alignmentVec)); //ccw is positive
anglePitch *= -1;
angleRoll *= -1;
double roll_deg = Math.Round(angleRoll / Math.PI * 180);
double pitch_deg = Math.Round(anglePitch / Math.PI * 180);
//---Angle controller
double rollSpeed = Math.Round(angleRoll, 2);
double pitchSpeed = Math.Round(anglePitch, 2);
//---Enforce rotation speed limit
if (Math.Abs(rollSpeed) + Math.Abs(pitchSpeed) > 2 * Math.PI)
{
double scale = 2 * Math.PI / (Math.Abs(rollSpeed) + Math.Abs(pitchSpeed));
rollSpeed *= scale;
pitchSpeed *= scale;
}
ApplyGyroOverride(pitchSpeed, 0, -rollSpeed);
}
else
{
foreach (IMyGyro thisGyro in gyros)
{
thisGyro.GyroOverride = false;
}
}
}
private void CalcWorldSpeed()
{
Vector3D linearVelocity = Vector3D.Normalize(cockpit.GetShipVelocities().LinearVelocity);
Vector3D gravity = -Vector3D.Normalize(cockpit.GetNaturalGravity());
worldSpeedForward = Helpers.NotNan(Vector3D.Dot(linearVelocity, Vector3D.Cross(gravity, cockpit.WorldMatrix.Right)) * cockpit.GetShipSpeed());
worldSpeedRight = Helpers.NotNan(Vector3D.Dot(linearVelocity, Vector3D.Cross(gravity, cockpit.WorldMatrix.Forward)) * cockpit.GetShipSpeed());
worldSpeedUp = Helpers.NotNan(Vector3D.Dot(linearVelocity, gravity) * cockpit.GetShipSpeed());
}
public void Main(string args = "START")
{
Config config = new Config(Me.CustomData);
config.Set(ref thrustersGroupName, "thrustersGroupName");
config.Set(ref referenceBlockName, "referenceBlockName");
config.Set(ref lcdSearchName, "lcdSearchName");
config.Set <double>(ref marginOfErrorThrust, "marginOfErrorThrust");
config.Set <double>(ref targetSpeed, "targetSpeed");
config.Set <double>(ref targetSpeedVariation, "targetSpeedVariation");
config.Set <double>(ref gravityTreshold, "gravityTreshold");
controlBlock = GridTerminalSystem.GetBlockWithName(referenceBlockName) as IMyShipController;
lcds = SearchBlocksWithName <IMyTextPanel>(lcdSearchName);
if (args == "START")
{
Runtime.UpdateFrequency = UpdateFrequency.Update10;
reachedTopSpeedOnce = false;
turnAndBurn = null;
}
lcds.ForEach(lcd => {
lcd.WritePublicTitle("Launch control");
lcd.WritePublicText(""); // Clear LCD
});
if (controlBlock == null)
{
WriteLine("No control block found on grid.");
WriteLine("Terminating script.");
return;
}
thrusters = GetBlocksInGroup <IMyThrust>(thrustersGroupName);
thrustController = new ThrustController(thrusters);
gyros = GetBlocksOfType <IMyGyro>();
gyroController = new GyroController(controlBlock, gyros, Base6Directions.Direction.Down, 0.8);
gravity = controlBlock.GetNaturalGravity();
gravityStrength = gravity.Length();
var escaped = gravityStrength <= gravityTreshold;
gravity.Normalize();
if (gravityStrength != 0)
{
lastObservedGravity = gravity;
}
if (thrusters == null || thrusters.Count == 0)
{
WriteLine($"No thrusters found in \"{thrustersGroupName}\" group.");
WriteLine("Terminating script.");
return;
}
speed = controlBlock.GetShipSpeed();
if (speed > targetSpeed)
{
reachedTopSpeedOnce = true;
}
WriteLine($"Ship speed: {Math.Round(speed, 1)} m/s");
WriteLine($"Target: {Math.Round(targetSpeed, 1)} m/s");
if (!escaped)
{
ApplyThrust();
gyroController.Align(gravity);
angle = Math.Acos(
Vector3D.Dot(
Vector3D.Normalize(controlBlock.GetNaturalGravity()),
Vector3D.Normalize(-controlBlock.GetShipVelocities().LinearVelocity)
)
) * 180 / Math.PI;
WriteLine($"Angle deviation: {Math.Round(angle)}°");
}
if (escaped)
{
if (turnAndBurn == null)
{
thrustController.Stop();
SetDampeners(false);
}
turnAndBurn = gyroController.Align(lastObservedGravity, Base6Directions.Direction.Up) ? "aligned" : "started";
WriteLine($"Turn and burn: {turnAndBurn}");
}
if (args == "STOP" || (escaped && turnAndBurn == "aligned"))
{
thrustController.Stop();
gyroController.Stop();
SetDampeners(true);
Runtime.UpdateFrequency = UpdateFrequency.None;
ClearOutput();
WriteLine("Launch control ended.");
}
}
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);
}
public void LcdWriter()
{
if (ticks % 31 == 0)
{
lcdCounter++;
}
if (lcdCounter > 6)
{
lcdCounter = 0;
}
/*===========| LCD's |===========*/
if (sensor.Main())
{
lcd.SetDirection(LCD_CLOSESTPERSON, " " + sensor.DistanceToClosest.ToString(), sensor.GetVector2());
if (MISCINFO)
{
//misc entity info
MyDetectedEntityInfo temp = sensor.closest;
var sb = new StringBuilder();
if (SCROLLING)
{
sb.AppendLine("Closest info:");
sb.AppendLine("Name: " + temp.Name);
sb.AppendLine("Speed: " + Math.Round(temp.Velocity.Length() * 2.23693629, 2) + " mph");
sb.AppendLine("Size: " + Math.Round(temp.BoundingBox.Size.Length() * 3.2808399, 2) + "ft");
}
else if (NOSCROLL)
{
sb.AppendLine("Closest info:");
sb.AppendLine("Name: " + temp.Name);
sb.AppendLine("Speed: " + Math.Round(temp.Velocity.Length() * 3.6, 2) + " km/h");
sb.AppendLine("Size: " + Math.Round(temp.BoundingBox.Size.Length(), 2) + "m");
}
else
{
sb.AppendLine("Closest info:");
sb.AppendLine("Name: " + temp.Name);
sb.AppendLine("Speed: " + Math.Round(temp.Velocity.Length(), 2) + " m/s");
sb.AppendLine("Size: " + Math.Round(temp.BoundingBox.Size.Length(), 2) + "m");
}
lcd.WriteTo(LCD_MISCINFO, sb.ToString());
}
}
if (!MISCINFO)
{
var sb = new StringBuilder();
sb.AppendLine("Craft info:");
if (SCROLLING)
{
if (lcdCounter == 0)
{
sb.AppendLine("Friction: " + Math.Round(wheel.GetCurrentFriction()).ToString() + "%");
}
if (lcdCounter == 1)
{
sb.AppendLine("Downforce: " + Math.Round(downforce.GetDownForcePercent(), 2).ToString() + "%");
}
if (lcdCounter == 2)
{
sb.AppendLine("Stopwatch:");
}
if (lcdCounter == 3)
{
sb.AppendLine("Time: " + stopwatch.GetTime() + "s");
}
if (lcdCounter == 4)
{
sb.AppendLine("Avg speed: " + Math.Round(stopwatch.GetSpeedAvg() * 3.6, 1) + "km/h");
}
if (lcdCounter == 5)
{
sb.AppendLine("Travelled distance: " + Math.Round(stopwatch.GetDistance() / 1000.0, 3) + "km");
}
}
else if (NOSCROLL)
{
sb.AppendLine("Craft info:");
sb.AppendLine("Friction: " + Math.Round(wheel.GetCurrentFriction()).ToString() + "%");
sb.AppendLine("Downforce: " + Math.Round(downforce.GetDownForcePercent(), 2).ToString() + "%");
sb.AppendLine("Stopwatch:");
sb.AppendLine("Time: " + stopwatch.GetTime() + "s");
sb.AppendLine("Avg speed: " + Math.Round(stopwatch.GetSpeedAvg() * 3.6, 1) + "km/h");
sb.AppendLine("Travelled distance: " + Math.Round(stopwatch.GetDistance() / 1000.0, 3) + "km");
}
else
{
sb.AppendLine("Craft info:");
sb.AppendLine("Friction: " + Math.Round(wheel.GetCurrentFriction()).ToString() + "%");
sb.AppendLine("Downforce: " + Math.Round(downforce.GetDownForcePercent(), 2).ToString() + "%");
sb.AppendLine("Stopwatch");
sb.AppendLine("Time: " + stopwatch.GetTime() + "s");
sb.AppendLine("Avg speed: " + Math.Round(stopwatch.GetSpeedAvg(), 2) + "m/s");
sb.AppendLine("Travelled distance: " + stopwatch.GetDistance() + "m");
}
if (lcdCounter == 6 || NOSCROLL)
{
sb.AppendLine("SnowMode: " + wheel.snowMode.ToString());
}
lcd.WriteTo(LCD_MISCINFO, sb.ToString());
}
lcd.WriteTo(LCD_SPEDOMETER, Math.Round(rc.GetShipSpeed() * 3.6, 2).ToString() + "\nkm/h");
}
