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.");
}
