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