// Compare current weather against list of 'bad' weathers.
// If weather is bad, and sensor not triggered, then trigger it
// If weather is good, and sensor is triggered, then un-trigger it
private void CheckBadWeather(IMySensorBlock sensor_block)
{
// Get current weather
MyAPIGateway.Session.WeatherEffects.GetWeather(sensor_block.GetPosition(), out MyObjectBuilder_WeatherEffect weatherEffect);
// Create List of 'bad' weathers
string[] bad_weathers = { "badweather1", "badweather2", "etc", };
List <string> bad_weathers_list = new List <string>(bad_weathers);
// Compare current weather against bad weather
if ((bad_weathers_list.Contains(weatherEffect.ToString()) == true) && (bad_weather_active == false))
{
bad_weather_active = true;
TriggerSensor(sensor_block, true);
}
else if ((bad_weathers_list.Contains(weatherEffect.ToString()) == false) && (bad_weather_active == true))
{
bad_weather_active = false;
TriggerSensor(sensor_block, false);
}
else
{
return;
}
}
public void updateNearbyCollisionData(IMySensorBlock sensor)
{
if (!sensor.LastDetectedEntity.IsEmpty())
{
MyDetectedEntityInfo entity = sensor.LastDetectedEntity;
if (!this.nearbyEntities.Any(val => val.id == entity.EntityId))
{
DetectedEntity tmp = new DetectedEntity();
tmp.id = entity.EntityId;
tmp.name = entity.Name;
tmp.position = entity.Position;
tmp.lastSeen = Communication.getTimestamp();
tmp.distance = this.getDistanceFrom(entity.Position, sensor.GetPosition());
tmp.type = entity.Type;
this.nearbyEntities.Add(tmp);
}
else
{
for (int i = 0; i < this.nearbyEntities.Count; i++)
{
DetectedEntity nearEntity = this.nearbyEntities[i];
if (nearEntity.id == entity.EntityId)
{
DetectedEntity tmp = new DetectedEntity();
tmp.id = entity.EntityId;
tmp.name = entity.Name;
tmp.position = entity.Position;
tmp.entityInfo = entity;
tmp.lastSeen = Communication.getTimestamp();
tmp.distance = this.getDistanceFrom(entity.Position, sensor.GetPosition());
tmp.type = entity.Type;
this.nearbyEntities[i] = tmp;
break;
}
}
}
}
else
{
List <DetectedEntity> tmp = new List <DetectedEntity>();
for (int i = 0; i < this.nearbyEntities.Count; i++)
{
DetectedEntity nearEntity = this.nearbyEntities[i];
if (Communication.getTimestamp() - nearEntity.lastSeen < 7 * 24 * 60 * 60) // Last seen in 7 days?
{
tmp.Add(nearEntity);
}
}
this.nearbyEntities = tmp;
}
}
public void AimTarget_sensor(IMySensorBlock sensor)
{
string[] vs = RArm.myMotor1.MyMotor.CustomData.Split(',');
var str = sensor.CustomData.Split(',');
var vector = new Vector3D(double.Parse(str[0]), double.Parse(str[1]), double.Parse(str[2]));
Vector3D worldDirection = vector - sensor.GetPosition();
Vector3D direction = Vector3D.TransformNormal(worldDirection, MatrixD.Transpose(sensor.WorldMatrix));
vs[1] = MathHelperD.ToDegrees(-Math.Atan2(direction.Z, direction.Y)).ToString();
RArm.myMotor1.MyMotor.CustomData = vs[0] + "," + vs[1] + ',' + vs[2];
vs = RArm.myMotor2.MyMotor.CustomData.Split(',');
vs[1] = "0"; //MathHelperD.ToDegrees(Math.Atan2(direction.X, direction.Y)).ToString();
RArm.myMotor2.MyMotor.CustomData = vs[0] + "," + vs[1] + ',' + vs[2];
}
void SensorSetToShip(IMyTerminalBlock tb1, float fLeft, float fRight, float fUp, float fDown, float fFront, float fBack)
{
// need to use world matrix to get orientation correctly
IMySensorBlock sb1 = tb1 as IMySensorBlock;
// Echo("SensorSetShip()");
int i1 = 0;
for (; i1 < sensorInfos.Count; i1++)
{
if (sensorInfos[i1].EntityId == sb1.EntityId)
{
break;
}
}
if (i1 < sensorInfos.Count)
{
// Echo("Using cached location information");
// we found cached info
float fSet = 0;
if (fLeft < 0)
{
fSet = -fLeft;
}
else
{
fSet = (float)Math.Abs(fLeft + Math.Abs(sensorInfos[i1].DistanceLeft));
}
sb1.LeftExtend = Math.Max(fSet, 1.0f);
if (fRight < 0)
{
fSet = -fRight;
}
else
{
fSet = (float)Math.Abs(fRight + Math.Abs(sensorInfos[i1].DistanceRight));
}
sb1.RightExtend = Math.Max(fSet, 1.0f);
if (fUp < 0)
{
fSet = -fUp;
}
else
{
fSet = (float)Math.Abs(fUp + Math.Abs(sensorInfos[i1].DistanceUp));
}
sb1.TopExtend = Math.Max(fSet, 1.0f);
if (fDown < 0)
{
fSet = -fDown;
}
else
{
fSet = (float)Math.Abs(fDown + Math.Abs(sensorInfos[i1].DistanceDown));
}
sb1.BottomExtend = Math.Max(fSet, 1.0f);
if (fFront < 0)
{
fSet = -fFront;
}
else
{
fSet = (float)Math.Abs(fFront + Math.Abs(sensorInfos[i1].DistanceFront));
}
sb1.FrontExtend = Math.Max(fSet, 1.0f);
if (fBack < 0)
{
fSet = -fBack;
}
else
{
fSet = (float)Math.Abs(fBack + Math.Abs(sensorInfos[i1].DistanceBack));
}
sb1.BackExtend = Math.Max(fSet, 1.0f);
}
else
{
OrientedBoundingBoxFaces orientedBoundingBox = new OrientedBoundingBoxFaces(shipOrientationBlock);
Vector3D vFTL;
Vector3D vFBL;
Vector3D vFTR;
Vector3D vFBR;
Vector3D vBTL;
Vector3D vBBL;
Vector3D vBTR;
Vector3D vBBR;
Vector3D[] points = new Vector3D[4];
orientedBoundingBox.GetFaceCorners(OrientedBoundingBoxFaces.LookupFront, points); // front output order is BL, BR, TL, TR
vFBL = points[0];
vFBR = points[1];
vFTL = points[2];
vFTR = points[3];
orientedBoundingBox.GetFaceCorners(OrientedBoundingBoxFaces.LookupBack, points); // face 4=back output order is BL, BR, TL, TR
vBBL = points[0];
vBBR = points[1];
vBTL = points[2];
vBTR = points[3];
debugGPSOutput("FBL", vFBL);
debugGPSOutput("FBR", vFBR);
debugGPSOutput("FTL", vFTL);
debugGPSOutput("FTR", vFTR);
debugGPSOutput("BBL", vBBL);
debugGPSOutput("BBR", vBBR);
debugGPSOutput("BTL", vBTL);
debugGPSOutput("BTR", vBTR);
if (sb1 == null)
{
return;
}
Echo(sb1.CustomName);
Vector3D vPos = sb1.GetPosition();
double distanceFront = PlanarDistance(vPos, vFBL, vFBR, vFTR);
Echo("DistanceFront=" + distanceFront.ToString("0.00"));
Vector3D vFront = vPos + sb1.WorldMatrix.Forward * distanceFront;
debugGPSOutput("FRONT", vFront);
double distanceBack = PlanarDistance(vPos, vBBL, vBBR, vBTR);
Echo("DistanceBack=" + distanceBack.ToString("0.00"));
Vector3D vBack = vPos + sb1.WorldMatrix.Forward * distanceBack; // distance is negative for 'back'
debugGPSOutput("BACK", vBack);
double distanceLeft = PlanarDistance(vPos, vFBL, vFTL, vBBL);
double distanceRight = PlanarDistance(vPos, vFBR, vFTR, vBBR);
double distanceUp = PlanarDistance(vPos, vFTL, vFTR, vBTL);
double distanceDown = PlanarDistance(vPos, vFBL, vFBR, vBBR);
float fSet = 0;
fSet = (float)Math.Abs(fLeft + Math.Abs(distanceLeft));
sb1.LeftExtend = Math.Max(fSet, 1.0f);
fSet = (float)Math.Abs(fRight + Math.Abs(distanceRight));
sb1.RightExtend = Math.Max(fSet, 1.0f);
fSet = (float)Math.Abs(fUp + Math.Abs(distanceUp));
sb1.TopExtend = Math.Max(fSet, 1.0f);
fSet = (float)Math.Abs(fDown + Math.Abs(distanceDown));
sb1.BottomExtend = Math.Max(fSet, 1.0f);
fSet = (float)Math.Abs(fFront + Math.Abs(distanceFront));
sb1.FrontExtend = Math.Max(fSet, 1.0f);
fSet = (float)Math.Abs(fBack + Math.Abs(distanceBack));
sb1.BackExtend = Math.Max(fSet, 1.0f);
}
/*
* //x=width, y=height, z=back/forth. (fw=+z) (right=-y)
*
* float fScale = 2.5f;
* if (tb1.CubeGrid.GridSizeEnum == MyCubeSize.Small)
* {
* Echo("Small Grid Ship");
* fScale = 0.5f;
* }
* else Echo("Large Grid Ship");
* float fXOffset = sb1.Position.X * fScale; // small grid only?
* float fYOffset = sb1.Position.Y * fScale;
* float fZOffset = sb1.Position.Z * fScale;
* Echo("SB.x.y.z=" + fXOffset.ToString("0.0") + ":" + fYOffset.ToString("0.0") + ":" + fZOffset.ToString("0.0"));
*
* // Echo("MIN=" + Me.CubeGrid.Min.ToString() + "\nMAX:" + Me.CubeGrid.Max.ToString());
* // TODO: need to use grid orientation to main orientation block
* // BUG: Assumes orientation is SAME as main orientation block
* float fSet;
* fSet = (float)(shipDim.WidthInMeters() / 2 - fXOffset + fLeft);
* sb1.LeftExtend = Math.Max(fSet, 1.0f);
* fSet = (float)(shipDim.WidthInMeters() / 2 + fXOffset + fRight);
* sb1.RightExtend = Math.Max(fSet, 1.0f);
*
* fSet = (float)(shipDim.HeightInMeters() / 2 - fYOffset + fUp);
* sb1.TopExtend = Math.Max(fSet, 1.0f);
* fSet = (float)(shipDim.HeightInMeters() / 2 + fYOffset + fDown);
* sb1.BottomExtend = Math.Max(fSet, 1.0f);
* fSet = (float)(shipDim.LengthInMeters() + fZOffset + fFront);
* sb1.FrontExtend = Math.Max(fSet, 1.0f);
* fSet = (float)(shipDim.LengthInMeters() - fZOffset + fBack);
* sb1.BackExtend = Math.Max(fSet, 1.0f);
*/
sb1.Enabled = true;
}
void doModeFindOre()
{
List <IMySensorBlock> aSensors = null;
IMySensorBlock sb;
IMySensorBlock sb2;
StatusLog("clear", textPanelReport);
StatusLog(moduleName + ":FindOre", textPanelReport);
Echo("current_state=" + current_state.ToString());
double maxThrust = calculateMaxThrust(thrustForwardList);
Echo("maxThrust=" + maxThrust.ToString("N0"));
MyShipMass myMass;
myMass = ((IMyShipController)gpsCenter).CalculateShipMass();
double effectiveMass = myMass.PhysicalMass;
Echo("effectiveMass=" + effectiveMass.ToString("N0"));
double maxDeltaV = (maxThrust) / effectiveMass;
Echo("maxDeltaV=" + maxDeltaV.ToString("0.00"));
Echo("Cargo=" + cargopcent.ToString() + "%");
Echo("velocity=" + velocityShip.ToString("0.00"));
Echo("miningElapsedMs=" + miningElapsedMs.ToString("0.00"));
/*
* Echo("#Sensors=" + sensorsList.Count);
* Echo("width=" + shipDim.WidthInMeters().ToString("0.0"));
* Echo("height=" + shipDim.HeightInMeters().ToString("0.0"));
* Echo("length=" + shipDim.LengthInMeters().ToString("0.0"));
*/
IMyTextPanel txtPanel = getTextBlock("Sensor Report");
StatusLog("clear", txtPanel);
// TODO: Check maxDeltaV. If <0.25, then we can't move..
// so turn around at 0.50 or less.. (and still use cargopcent?)
if (bValidAsteroid)
{
debugGPSOutput("Pre-Valid Ast", vTargetAsteroid);
}
// if(vExpectedExit.AbsMax()>.5)
{
Vector3D vT = gpsCenter.GetPosition() + vExpectedExit * 150;
debugGPSOutput("ExpectedExit", vT);
}
switch (current_state)
{
case 0:
current_state = 10;
bWantFast = true;
if (sensorsList.Count < 2)
{
StatusLog(OurName + ":" + moduleName + " Find Ore: Not Enough Sensors!", textLongStatus, true);
setMode(MODE_ATTENTION);
}
sensorsList[0].DetectAsteroids = true;
sensorsList[1].DetectAsteroids = true;
bValidAsteroid = false;
bValidExit = false;
ResetMotion();
// turnDrillsOff();
turnEjectorsOff();
break;
case 10:
//sb = sensorsList[0];
sleepAllSensors();
setSensorShip(sensorsList[0], 2, 2, 2, 2, 2, 2);
// setSensorShip(sensorsList[1], 50, 50, 50, 50, 50, 0);
miningElapsedMs = 0;
current_state = 11;
break;
case 11:
{
miningElapsedMs += Runtime.TimeSinceLastRun.TotalMilliseconds;
if (miningElapsedMs < 1)
{
return;
}
aSensors = activeSensors();
bool bFoundAsteroid = false;
for (int i = 0; i < aSensors.Count; i++)
{
IMySensorBlock s = aSensors[i] as IMySensorBlock;
StatusLog(aSensors[i].CustomName + " ACTIVE!", txtPanel);
Echo(aSensors[i].CustomName + " ACTIVE!");
List <MyDetectedEntityInfo> lmyDEI = new List <MyDetectedEntityInfo>();
s.DetectedEntities(lmyDEI);
StatusLog("#DetectedEntities=" + lmyDEI.Count, txtPanel);
strbMining.Clear();
strbMining.Append("Sensor Position:" + s.GetPosition().ToString());
strbMining.AppendLine();
for (int j = 0; j < lmyDEI.Count; j++)
{
strbMining.Append("Name: " + lmyDEI[j].Name);
strbMining.AppendLine();
strbMining.Append("Type: " + lmyDEI[j].Type);
strbMining.AppendLine();
if (lmyDEI[j].Type == MyDetectedEntityType.Asteroid)
{
addDetectedEntity(lmyDEI[j]);
StatusLog("Found Asteroid!", txtPanel);
bFoundAsteroid = true;
currentAst.EntityId = lmyDEI[j].EntityId;
currentAst.BoundingBox = lmyDEI[j].BoundingBox;
bValidAsteroid = true;
vTargetAsteroid = lmyDEI[j].Position;
}
}
StatusLog(strbMining.ToString(), txtPanel);
}
if (bFoundAsteroid)
{
current_state = 20; // inside
}
else
{
current_state = 100;
}
}
break;
case 20:
{
// started find ore while 'inside' asteroid.
// point towards exit
vExpectedExit = gpsCenter.GetPosition() - currentAst.Position;
vExpectedExit.Normalize();
bValidExit = true;
current_state = 31;
}
break;
case 31:
sb = sensorsList[0];
sb2 = sensorsList[1];
sleepAllSensors();
setSensorShip(sb, 0, 0, 0, 0, 45, 0);
setSensorShip(sb2, 5, 5, 5, 5, 0, 15);
current_state = 32;
miningElapsedMs = 0;
break;
case 32:
miningElapsedMs += Runtime.TimeSinceLastRun.TotalMilliseconds;
if (miningElapsedMs < 1)
{
return; // delay for sensor settling
}
current_state = 35;
break;
case 35:
{
aSensors = activeSensors();
// bValidAsteroid = false;
bool bLocalAsteroid = false;
for (int i = 0; i < aSensors.Count; i++)
{
IMySensorBlock s = aSensors[i] as IMySensorBlock;
List <MyDetectedEntityInfo> lmyDEI = new List <MyDetectedEntityInfo>();
s.DetectedEntities(lmyDEI);
for (int j = 0; j < lmyDEI.Count; j++)
{
if (lmyDEI[j].Type == MyDetectedEntityType.Asteroid)
{
addDetectedEntity(lmyDEI[j]);
bLocalAsteroid = true;
if (!bValidAsteroid)
{
Echo("Found New Asteroid!");
bValidAsteroid = true;
vTargetAsteroid = lmyDEI[j].Position;
}
currentAst.EntityId = lmyDEI[j].EntityId;
currentAst.BoundingBox = lmyDEI[j].BoundingBox;
}
// need to detect other ships and avoid
}
}
if (!bLocalAsteroid)
{ // we have exited the asteroid.
ResetMotion();
// turnDrillsOff();
if (cargopcent > cargopctlowwater || maxDeltaV < (fTargetMiningmps / 2))
{
// we need to dump our contents
turnEjectorsOn();
}
current_state = 300;
bWantFast = true;
}
else
{ // we are inside asteroid
turnEjectorsOn();
GyroMain("forward", vExpectedExit, gpsCenter);
// blockApplyAction(ejectorList, "OnOff_On");
if (maxDeltaV < (fTargetMiningmps / 2) || cargopcent > cargopcthighwater && !bWaitingCargo) //
{
ResetMotion();
// turnDrillsOff();
turnEjectorsOn();
bWaitingCargo = true;
}
if (bWaitingCargo)
{ // continue to wait
// need to check how much stone we have.. if zero(ish), then we're full.. go exit.
doCargoOreCheck();
if (currentStoneAmount() < 15)
{
// we are full and not much stone ore in us...
ResetMotion();
// turnDrillsOff();
turnEjectorsOff();
setMode(MODE_EXITINGASTEROID);
doTriggerMain();
}
// TODO: Needs time-out
Echo("Cargo above low water: Waiting");
if (maxDeltaV > (fTargetMiningmps / 2) && cargopcent < cargopctlowwater)
{
bWaitingCargo = false; // can now move.
}
}
else
{
// GyroMain("forward", vExpectedExit, gpsCenter);
turnDrillsOn();
mineMoveForward(fTargetMiningmps, fAbortmps);
bWantFast = true;
}
}
}
break;
case 100:
turnDrillsOff();
sleepAllSensors();
sb = sensorsList[0];
setSensorShip(sb, 0, 0, 0, 0, 50, 0);
current_state = 101;
miningElapsedMs = 0;
break;
case 101:
miningElapsedMs += Runtime.TimeSinceLastRun.TotalMilliseconds;
if (miningElapsedMs < 1)
{
return; // delay for sensor settling
}
current_state++;
break;
case 102:
{
aSensors = activeSensors();
bValidAsteroid = false;
for (int i = 0; i < aSensors.Count; i++)
{
IMySensorBlock s = aSensors[i] as IMySensorBlock;
StatusLog(aSensors[i].CustomName + " ACTIVE!", txtPanel);
Echo(aSensors[i].CustomName + " ACTIVE!");
List <MyDetectedEntityInfo> lmyDEI = new List <MyDetectedEntityInfo>();
s.DetectedEntities(lmyDEI);
StatusLog("#DetectedEntities=" + lmyDEI.Count, txtPanel);
strbMining.Clear();
strbMining.Append("Sensor Position:" + s.GetPosition().ToString());
strbMining.AppendLine();
for (int j = 0; j < lmyDEI.Count; j++)
{
strbMining.Append("Name: " + lmyDEI[j].Name);
strbMining.AppendLine();
strbMining.Append("Type: " + lmyDEI[j].Type);
strbMining.AppendLine();
if (lmyDEI[j].Type == MyDetectedEntityType.Asteroid)
{
addDetectedEntity(lmyDEI[j]);
StatusLog("Found Asteroid!", txtPanel);
bValidAsteroid = true;
vTargetAsteroid = lmyDEI[j].Position;
currentAst.EntityId = lmyDEI[j].EntityId;
currentAst.BoundingBox = lmyDEI[j].BoundingBox;
}
}
StatusLog(strbMining.ToString(), txtPanel);
// else current_state = 100;
}
// if (!bValidAsteroid)
{
// not in sensor range. start checking cameras
double scandist = 500;
/*
* if (bValidAsteroid)
* {
* scandist = (gpsCenter.GetPosition() - vTargetAsteroid).Length();
* }
*/
if (doCameraScan(cameraForwardList, scandist))
{ // we scanned
if (!lastDetectedInfo.IsEmpty())
{ // we hit something
if (lastDetectedInfo.Type == MyDetectedEntityType.Asteroid)
{
addDetectedEntity(lastDetectedInfo);
vTargetAsteroid = (Vector3D)lastDetectedInfo.HitPosition;
bValidAsteroid = true;
vExpectedExit = vTargetAsteroid - gpsCenter.GetPosition();
vExpectedExit.Normalize();
bValidExit = true;
}
}
else
{
// no asteroid detected. Check surroundings for one.
current_state = 110;
bValidExit = false;
}
}
}
if (bValidExit)
{
current_state = 120; //found asteroid ahead
}
}
break;
case 110:
{
sleepAllSensors();
setSensorShip(sensorsList[0], 50, 50, 50, 50, 50, 50);
miningElapsedMs = 0;
current_state = 111;
}
break;
case 111:
miningElapsedMs += Runtime.TimeSinceLastRun.TotalMilliseconds;
if (miningElapsedMs < 1)
{
return; // delay for sensor settling
}
current_state++;
break;
case 112:
{
aSensors = activeSensors();
// bValidAsteroid = false;
for (int i = 0; i < aSensors.Count; i++)
{
IMySensorBlock s = aSensors[i] as IMySensorBlock;
StatusLog(aSensors[i].CustomName + " ACTIVE!", txtPanel);
Echo(aSensors[i].CustomName + " ACTIVE!");
List <MyDetectedEntityInfo> lmyDEI = new List <MyDetectedEntityInfo>();
s.DetectedEntities(lmyDEI);
StatusLog("#DetectedEntities=" + lmyDEI.Count, txtPanel);
strbMining.Clear();
strbMining.Append("Sensor Position:" + s.GetPosition().ToString());
strbMining.AppendLine();
for (int j = 0; j < lmyDEI.Count; j++)
{
strbMining.Append("Name: " + lmyDEI[j].Name);
strbMining.AppendLine();
strbMining.Append("Type: " + lmyDEI[j].Type);
strbMining.AppendLine();
if (lmyDEI[j].Type == MyDetectedEntityType.Asteroid)
{
addDetectedEntity(lmyDEI[j]);
StatusLog("Found Asteroid!", txtPanel);
bValidAsteroid = true;
vTargetAsteroid = lmyDEI[j].Position;
currentAst.EntityId = lmyDEI[j].EntityId;
currentAst.BoundingBox = lmyDEI[j].BoundingBox;
}
}
StatusLog(strbMining.ToString(), txtPanel);
// else current_state = 100;
}
if (bValidAsteroid)
{
bWantFast = true;
if (GyroMain("forward", vTargetAsteroid - gpsCenter.GetPosition(), gpsCenter))
{
// we are aimed
current_state = 100;
}
}
}
break;
case 120:
{ // approach
if (!bValidAsteroid)
{
current_state = 10;
return;
}
bWantFast = true;
// we should have asteroid in front.
bool bAimed = true;
Echo(bValidExit.ToString() + " " + Vector3DToString(vExpectedExit));
bAimed = GyroMain("forward", vExpectedExit, gpsCenter);
if (bAimed)
{
if (doCameraScan(cameraForwardList, 500))
{
if (!lastDetectedInfo.IsEmpty())
{
// we have a target
// if (lmyDEI[j].Type == MyDetectedEntityType.Asteroid)
{
Vector3D vTarget = (Vector3D)lastDetectedInfo.HitPosition;
double distance = (vTarget - lastCamera.GetPosition()).Length();
Echo("Distance=" + distance.ToString());
if (distance < 15)
{
vLastContact = gpsCenter.GetPosition();
if (!bValidInitialContact)
{
vInitialContact = vLastContact;
bValidInitialContact = true;
}
current_state = 31;
ResetMotion();
}
else
{
mineMoveForward(fApproachMps, fApproachAbortMps);
}
}
// bValidExit = true;
// vExpectedExit = vTarget - gpsCenter.GetPosition();
}
else
{
/*
* // we scanned, but didn't hit anything. it's likely a donut
* double distance = (vTargetAsteroid - gpsCenter.GetPosition()).Length();
* Echo("Distance=" + distance.ToString());
* mineMoveForward(fTargetMiningmps,fAbortmps);
* if (distance < 5)
* {
* vExpectedExit = vTargetAsteroid - gpsCenter.GetPosition();
* current_state = 31;
* ResetMotion();
* }
*/
setMode(MODE_ATTENTION);
}
}
// ELSE no scan available.. just wait
else
{
Echo("No Camera SCAN!");
}
}
else
{
Echo("Aiming");
}
// BUG: Gets stuck in state if nothing hit.. (ie, donut)
}
break;
case 300:
{
// we have exitted the asteroid. Prepare for another run or to go dock
Echo("Exitted!");
ResetMotion();
sleepAllSensors();
if (maxDeltaV < (fTargetMiningmps / 2) || cargopcent > cargopctlowwater)
{
setMode(MODE_DOCKING);
}
else
{
vLastExit = gpsCenter.GetPosition();
// dist = (vLastExit - vInitialExit).Length();
if (!bValidInitialExit)
{
vInitialExit = vLastExit;
bValidInitialExit = true;
}
setMode(MODE_SEARCHORIENT);
// prepare for another run.
}
break;
}
}
}
void doModeExitingAsteroid()
{
List <IMySensorBlock> aSensors = null;
IMySensorBlock sb;
StatusLog("clear", textPanelReport);
StatusLog(moduleName + ":Exiting!", textPanelReport);
Echo("current_state=" + current_state.ToString());
MyShipMass myMass;
myMass = ((IMyShipController)gpsCenter).CalculateShipMass();
double effectiveMass = myMass.PhysicalMass;
double maxThrust = calculateMaxThrust(thrustForwardList);
Echo("effectiveMass=" + effectiveMass.ToString("N0"));
Echo("maxThrust=" + maxThrust.ToString("N0"));
double maxDeltaV = (maxThrust) / effectiveMass;
Echo("maxDeltaV=" + maxDeltaV.ToString("0.00"));
Echo("Cargo=" + cargopcent.ToString() + "%");
Echo("velocity=" + velocityShip.ToString("0.00"));
Echo("#Sensors=" + sensorsList.Count);
Echo("width=" + shipDim.WidthInMeters().ToString("0.0"));
Echo("height=" + shipDim.HeightInMeters().ToString("0.0"));
Echo("length=" + shipDim.LengthInMeters().ToString("0.0"));
IMyTextPanel txtPanel = getTextBlock("Sensor Report");
StatusLog("clear", txtPanel);
/*
* 0 - Master Init
* 10 - Init sensors, turn drills on
* 11 - await sensor set
* 20 - turn around until aimed ->30
* 30 - move forward (out) until exit asteroid
*
* 40 when out, call for pickup
*
*/
switch (current_state)
{
case 0: //0 - Master Init
if (sensorsList.Count < 2)
{
StatusLog(OurName + ":" + moduleName + " Find Ore: Not Enough Sensors!", textLongStatus, true);
setMode(MODE_ATTENTION);
}
ResetMotion();
// turnDrillsOff();
turnEjectorsOn();
current_state = 10;
break;
case 10: //10 - Init sensors, turn drills on
sleepAllSensors();
setSensorShip(sensorsList[0], 2, 2, 2, 2, 15, 15);
// setSensorShip(sensorsList[1], 50, 50, 50, 50, 50, 0);
miningElapsedMs = 0;
current_state = 11;
break;
case 11: //11 - await sensor set
miningElapsedMs += Runtime.TimeSinceLastRun.TotalMilliseconds;
if (miningElapsedMs < 1)
{
return;
}
current_state = 20;
break;
case 20: //20 - turn around until aimed ->30
{
bWantFast = true;
turnDrillsOn();
// we want to turn on our horizontal axis as that should be the 'wide' one.
bool bAimed = false;
double yawangle = -999;
Echo("vTarget=" + Vector3DToString(vLastContact));
yawangle = CalculateYaw(vLastContact, gpsCenter);
Echo("yawangle=" + yawangle.ToString());
double aYawAngle = Math.Abs(yawangle);
bAimed = aYawAngle < .05;
float maxYPR = GyroControl.MaxYPR;
if (aYawAngle > 1.0)
{
maxYPR = maxYPR / 2;
}
DoRotate(yawangle, "Yaw", maxYPR);
/*
* //minAngleRad = 0.1f;
* bAimed=GyroMain("backward", vExpectedExit, gpsCenter);
*
* // minAngleRad = 0.01f;
* // GyroMain("backward", vExpectedExit, gpsCenter);
*/
if (bAimed)
{
mineMoveForward(fTargetMiningmps, fAbortmps);
}
bool bLocalAsteroid = false;
aSensors = activeSensors();
for (int i = 0; i < aSensors.Count; i++)
{
IMySensorBlock s = aSensors[i] as IMySensorBlock;
StatusLog(aSensors[i].CustomName + " ACTIVE!", txtPanel);
Echo(aSensors[i].CustomName + " ACTIVE!");
List <MyDetectedEntityInfo> lmyDEI = new List <MyDetectedEntityInfo>();
s.DetectedEntities(lmyDEI);
StatusLog("#DetectedEntities=" + lmyDEI.Count, txtPanel);
strbMining.Clear();
strbMining.Append("Sensor Position:" + s.GetPosition().ToString());
strbMining.AppendLine();
for (int j = 0; j < lmyDEI.Count; j++)
{
strbMining.Append("Name: " + lmyDEI[j].Name);
strbMining.AppendLine();
strbMining.Append("Type: " + lmyDEI[j].Type);
strbMining.AppendLine();
if (lmyDEI[j].Type == MyDetectedEntityType.Asteroid)
{
addDetectedEntity(lmyDEI[j]);
StatusLog("Found Asteroid!", txtPanel);
bLocalAsteroid = true;
}
}
StatusLog(strbMining.ToString(), txtPanel);
}
if (!bLocalAsteroid)
{
ResetMotion();
// turnDrillsOff();
sleepAllSensors();
current_state = 40;
}
}
break;
case 30: //30 - move forward (out) until exit asteroid
break;
case 40: //40 when out, call for pickup
{
turnDrillsOff();
// we should probably give hint to docking as to WHY we want to dock..
setMode(MODE_DOCKING);
}
break;
}
}
float fMinSensorSetting = 0.1f; //was 1.0f before 1.193.100
public void SensorSetToShip(IMySensorBlock sb1, float fLeft, float fRight, float fUp, float fDown, float fFront, float fBack)
{
if (sb1 == null)
{
_program.ErrorLog("SetSensorToShip: Null sensor");
return;
}
// _program.ErrorLog("SensorSetShip("+sb1.CustomName+")");
int iSensor = 0;
for (; iSensor < sensorInfos.Count; iSensor++)
{
if (sensorInfos[iSensor].EntityId == sb1.EntityId)
{
break;
}
}
if (iSensor < sensorInfos.Count)
{
// Echo("Using cached location information");
// we found cached info
float fSet = 0;
if (fLeft < 0)
{
fSet = -fLeft;
}
else
{
fSet = (float)Math.Abs(fLeft + Math.Abs(sensorInfos[iSensor].DistanceLeft));
}
sb1.LeftExtend = Math.Max(fSet, fMinSensorSetting);
if (fRight < 0)
{
fSet = -fRight;
}
else
{
fSet = (float)Math.Abs(fRight + Math.Abs(sensorInfos[iSensor].DistanceRight));
}
sb1.RightExtend = Math.Max(fSet, fMinSensorSetting);
if (fUp < 0)
{
fSet = -fUp;
}
else
{
fSet = (float)Math.Abs(fUp + Math.Abs(sensorInfos[iSensor].DistanceUp));
}
sb1.TopExtend = Math.Max(fSet, fMinSensorSetting);
if (fDown < 0)
{
fSet = -fDown;
}
else
{
fSet = (float)Math.Abs(fDown + Math.Abs(sensorInfos[iSensor].DistanceDown));
}
sb1.BottomExtend = Math.Max(fSet, fMinSensorSetting);
if (fFront < 0)
{
fSet = -fFront;
}
else
{
fSet = (float)Math.Abs(fFront + Math.Abs(sensorInfos[iSensor].DistanceFront));
}
sb1.FrontExtend = Math.Max(fSet, fMinSensorSetting);
if (fBack < 0)
{
fSet = -fBack;
}
else
{
fSet = (float)Math.Abs(fBack + Math.Abs(sensorInfos[iSensor].DistanceBack));
}
sb1.BackExtend = Math.Max(fSet, fMinSensorSetting);
}
else
{
OrientedBoundingBoxFaces orientedBoundingBox = new OrientedBoundingBoxFaces(shipController);
Vector3D vFTL;
Vector3D vFBL;
Vector3D vFTR;
Vector3D vFBR;
Vector3D vBTL;
Vector3D vBBL;
Vector3D vBTR;
Vector3D vBBR;
Vector3D[] points = new Vector3D[4];
orientedBoundingBox.GetFaceCorners(OrientedBoundingBoxFaces.LookupFront, points); // front output order is BL, BR, TL, TR
vFBL = points[0];
vFBR = points[1];
vFTL = points[2];
vFTR = points[3];
// _program.ErrorLog("vFBL=" + vFBL.ToString());
// _program.ErrorLog("vFBR=" + vFBR.ToString());
orientedBoundingBox.GetFaceCorners(OrientedBoundingBoxFaces.LookupBack, points); // face 4=back output order is BL, BR, TL, TR
vBBL = points[0];
vBBR = points[1];
vBTL = points[2];
vBTR = points[3];
// debugGPSOutput("FBL", vFBL);
// debugGPSOutput("FBR", vFBR);
// debugGPSOutput("FTL", vFTL);
// debugGPSOutput("FTR", vFTR);
// debugGPSOutput("BBL", vBBL);
// debugGPSOutput("BBR", vBBR);
// debugGPSOutput("BTL", vBTL);
// debugGPSOutput("BTR", vBTR);
// Echo(sb1.CustomName);
Vector3D vPos = sb1.GetPosition();
double distanceFront = PlanarDistance(vPos, vFBL, vFBR, vFTR);
// _program.ErrorLog("DistanceFront=" + distanceFront.ToString("0.00"));
Vector3D vFront = vPos + sb1.WorldMatrix.Forward * distanceFront;
// debugGPSOutput("FRONT", vFront);
double distanceBack = PlanarDistance(vPos, vBBL, vBBR, vBTR);
// _program.ErrorLog("DistanceBack=" + distanceBack.ToString("0.00"));
Vector3D vBack = vPos + sb1.WorldMatrix.Forward * distanceBack; // distance is negative for 'back'
// debugGPSOutput("BACK", vBack);
double distanceLeft = PlanarDistance(vPos, vFBL, vFTL, vBBL);
double distanceRight = PlanarDistance(vPos, vFBR, vFTR, vBBR);
double distanceUp = PlanarDistance(vPos, vFTL, vFTR, vBTL);
double distanceDown = PlanarDistance(vPos, vFBL, vFBR, vBBR);
float fSet = 0;
fSet = (float)Math.Abs(fLeft + Math.Abs(distanceLeft));
sb1.LeftExtend = Math.Max(fSet, fMinSensorSetting);
fSet = (float)Math.Abs(fRight + Math.Abs(distanceRight));
sb1.RightExtend = Math.Max(fSet, fMinSensorSetting);
fSet = (float)Math.Abs(fUp + Math.Abs(distanceUp));
sb1.TopExtend = Math.Max(fSet, fMinSensorSetting);
fSet = (float)Math.Abs(fDown + Math.Abs(distanceDown));
sb1.BottomExtend = Math.Max(fSet, fMinSensorSetting);
fSet = (float)Math.Abs(fFront + Math.Abs(distanceFront));
sb1.FrontExtend = Math.Max(fSet, fMinSensorSetting);
fSet = (float)Math.Abs(fBack + Math.Abs(distanceBack));
sb1.BackExtend = Math.Max(fSet, fMinSensorSetting);
}
sb1.Enabled = true;
}
void doModeMine()
{
List <IMySensorBlock> aSensors = null;
IMySensorBlock sb;
StatusLog("clear", textPanelReport);
StatusLog(moduleName + ":Mine!", textPanelReport);
Echo("current_state=" + current_state.ToString());
MyShipMass myMass;
myMass = ((IMyShipController)gpsCenter).CalculateShipMass();
double effectiveMass = myMass.PhysicalMass;
double maxThrust = calculateMaxThrust(thrustForwardList);
Echo("effectiveMass=" + effectiveMass.ToString("N0"));
Echo("maxThrust=" + maxThrust.ToString("N0"));
double maxDeltaV = (maxThrust) / effectiveMass;
Echo("maxDeltaV=" + maxDeltaV.ToString("0.00"));
Echo("Cargo=" + cargopcent.ToString() + "%");
Echo("velocity=" + velocityShip.ToString("0.00"));
Echo("#Sensors=" + sensorsList.Count);
Echo("width=" + shipDim.WidthInMeters().ToString("0.0"));
Echo("height=" + shipDim.HeightInMeters().ToString("0.0"));
Echo("length=" + shipDim.LengthInMeters().ToString("0.0"));
IMyTextPanel txtPanel = getTextBlock("Sensor Report");
StatusLog("clear", txtPanel);
// TODO: Check maxDeltaV. If <0.25, then we can't move..
// so turn around at 0.50 or less.. (and still use cargopcent?)
switch (current_state)
{
case 0:
current_state = 10;
bWantFast = true;
if (sensorsList.Count < 2)
{
StatusLog(OurName + ":" + moduleName + " Find Ore: Not Enough Sensors!", textLongStatus, true);
setMode(MODE_ATTENTION);
}
ResetMotion();
break;
case 10:
//sb = sensorsList[0];
sleepAllSensors();
setSensorShip(sensorsList[0], 2, 2, 2, 2, 2, 0);
// setSensorShip(sensorsList[1], 50, 50, 50, 50, 50, 0);
current_state = 11;
break;
case 11:
{
aSensors = activeSensors();
bool bFoundAsteroid = false;
for (int i = 0; i < aSensors.Count; i++)
{
IMySensorBlock s = aSensors[i] as IMySensorBlock;
StatusLog(aSensors[i].CustomName + " ACTIVE!", txtPanel);
Echo(aSensors[i].CustomName + " ACTIVE!");
List <MyDetectedEntityInfo> lmyDEI = new List <MyDetectedEntityInfo>();
s.DetectedEntities(lmyDEI);
StatusLog("#DetectedEntities=" + lmyDEI.Count, txtPanel);
strb.Clear();
strb.Append("Sensor Position:" + s.GetPosition().ToString());
strb.AppendLine();
for (int j = 0; j < lmyDEI.Count; j++)
{
strb.Append("Name: " + lmyDEI[j].Name);
strb.AppendLine();
strb.Append("Type: " + lmyDEI[j].Type);
strb.AppendLine();
if (lmyDEI[j].Type == MyDetectedEntityType.Asteroid)
{
addDetectedEntity(lmyDEI[j]);
StatusLog("Found Asteroid!", txtPanel);
bFoundAsteroid = true;
currentAst.EntityId = lmyDEI[j].EntityId;
currentAst.BoundingBox = lmyDEI[j].BoundingBox;
bValidAsteroid = true;
vTargetAsteroid = lmyDEI[j].Position;
}
}
StatusLog(strb.ToString(), txtPanel);
}
if (bFoundAsteroid)
{
current_state = 20; // inside
}
// should search using cameras..
// TODO:
else
{
current_state = 100;
}
}
break;
case 20:
// started find ore while 'inside' asteroid.
// point towards exit
vExpectedExit = gpsCenter.GetPosition() - currentAst.Position;
current_state = 31;
break;
case 31:
sb = sensorsList[0];
sleepAllSensors();
setSensorShip(sb, 0, 0, 0, 0, 25, 4);
current_state = 35;
break;
case 35:
{
aSensors = activeSensors();
bValidAsteroid = false;
for (int i = 0; i < aSensors.Count; i++)
{
IMySensorBlock s = aSensors[i] as IMySensorBlock;
List <MyDetectedEntityInfo> lmyDEI = new List <MyDetectedEntityInfo>();
s.DetectedEntities(lmyDEI);
for (int j = 0; j < lmyDEI.Count; j++)
{
if (lmyDEI[j].Type == MyDetectedEntityType.Asteroid)
{
addDetectedEntity(lmyDEI[j]);
Echo("Found Asteroid!");
bValidAsteroid = true;
vTargetAsteroid = lmyDEI[j].Position;
currentAst.EntityId = lmyDEI[j].EntityId;
currentAst.BoundingBox = lmyDEI[j].BoundingBox;
}
// need to detect other ships and avoid
}
}
if (!bValidAsteroid)
{ // we have exited the asteroid.
ResetMotion();
if (cargopcent > cargopctlowwater || maxDeltaV < fTargetMiningmps)
{
// we need to dump our contents
}
current_state = 10;
bWantFast = true;
}
else
{ // we are inside asteroid
turnEjectorsOn();
// blockApplyAction(ejectorList, "OnOff_On");
if (cargopcent > cargopcthighwater && !bWaitingCargo) //maxDeltaV<fTargetMiningmps ||
{
// need to check how much stone we have.. if zero, then we're full.. go exit.
// TODO:
ResetMotion();
turnDrillsOff();
// blockApplyAction(drillList, "OnOff_Off");
bWaitingCargo = true;
}
if (bWaitingCargo && cargopcent > cargopctlowwater)
{ // continue to wait
// TODO: Needs time-out
Echo("Cargo above highwater: Waiting");
// should check for stone amount...
}
else
{
bWaitingCargo = false;
GyroMain("forward", vExpectedExit, gpsCenter);
turnDrillsOn();
// blockApplyAction(drillList, "OnOff_On");
mineMoveForward();
bWantFast = true;
}
}
}
break;
case 100:
turnDrillsOff();
// blockApplyAction(drillList, "OnOff_Off");
sb = sensorsList[0];
setSensorShip(sb, 0, 0, 0, 0, 50, 0);
current_state = 101;
break;
case 101:
{
aSensors = activeSensors();
bValidAsteroid = false;
for (int i = 0; i < aSensors.Count; i++)
{
IMySensorBlock s = aSensors[i] as IMySensorBlock;
StatusLog(aSensors[i].CustomName + " ACTIVE!", txtPanel);
Echo(aSensors[i].CustomName + " ACTIVE!");
List <MyDetectedEntityInfo> lmyDEI = new List <MyDetectedEntityInfo>();
s.DetectedEntities(lmyDEI);
StatusLog("#DetectedEntities=" + lmyDEI.Count, txtPanel);
strb.Clear();
strb.Append("Sensor Position:" + s.GetPosition().ToString());
strb.AppendLine();
for (int j = 0; j < lmyDEI.Count; j++)
{
strb.Append("Name: " + lmyDEI[j].Name);
strb.AppendLine();
strb.Append("Type: " + lmyDEI[j].Type);
strb.AppendLine();
if (lmyDEI[j].Type == MyDetectedEntityType.Asteroid)
{
addDetectedEntity(lmyDEI[j]);
StatusLog("Found Asteroid!", txtPanel);
bValidAsteroid = true;
vTargetAsteroid = lmyDEI[j].Position;
currentAst.EntityId = lmyDEI[j].EntityId;
currentAst.BoundingBox = lmyDEI[j].BoundingBox;
}
}
StatusLog(strb.ToString(), txtPanel);
// else current_state = 100;
}
// if (!bValidAsteroid)
{
// not in sensor range. start checking cameras
if (doCameraScan(cameraForwardList, 5000))
{ // we scanned
if (!lastDetectedInfo.IsEmpty())
{ // we hit something
if (lastDetectedInfo.Type == MyDetectedEntityType.Asteroid)
{
addDetectedEntity(lastDetectedInfo);
vTargetAsteroid = (Vector3D)lastDetectedInfo.HitPosition;
bValidAsteroid = true;
vExpectedExit = vTargetAsteroid - gpsCenter.GetPosition();
}
}
}
}
if (bValidAsteroid)
{
current_state = 120; //found asteroid ahead
}
}
break;
case 120:
{
bWantFast = true;
// we should have asteroid in front.
bool bAimed = false;
Vector3D vVec = vTargetAsteroid - gpsCenter.GetPosition();
bAimed = GyroMain("forward", vVec, gpsCenter);
if (bAimed)
{
if (doCameraScan(cameraForwardList, 5000))
{
if (!lastDetectedInfo.IsEmpty())
{
// we have a target
Vector3D vTarget = (Vector3D)lastDetectedInfo.HitPosition;
double distance = (vTarget - gpsCenter.GetPosition()).Length();
Echo("Distance=" + distance.ToString());
mineMoveForward();
if (distance < 5)
{
current_state = 31;
ResetMotion();
}
}
else
{
// we scanned, but didn't hit anything. it's likely a donut
double distance = (vTargetAsteroid - gpsCenter.GetPosition()).Length();
Echo("Distance=" + distance.ToString());
mineMoveForward();
if (distance < 5)
{
current_state = 31;
ResetMotion();
}
}
}
else
{
Echo("No Camera SCAN!");
}
}
else
{
Echo("Aiming");
}
// BUG: Gets stuck in state if nothing hit.. (ie, donut)
// ELSE no scan available.. just wait
}
break;
}
}
public void Main(string argument, UpdateType updateSource)
{
mySensor.DetectedEntities(detectedEntityInfos);
text.WritePublicText("");
foreach (var item in detectedEntityInfos)
{
WriteText(item.Name + ":" + item.Position.ToString() + "\n");
}
if (detectedEntityInfos.Count == 0 || Me.CustomData == StaticCmd)
{
NowTargetEntityInfoID = 0;
Settledlist = new List <long>();
getTarget = false;
var vector = mySensor.GetPosition() + (mySensor.WorldMatrix.Forward * 500);
mySensor.CustomData = vector.X + "," + vector.Y + "," + vector.Z;
}
else
{
if (Me.CustomData == ChageCmd)
{
Me.CustomData = "";
foreach (var item in detectedEntityInfos)
{
if (!Settledlist.Contains(item.EntityId))
{
NowTargetEntityInfoID = item.EntityId;
Settledlist.Add(item.EntityId);
mySensor.CustomData = item.Position.X + "," + item.Position.Y + "," + item.Position.Z;
return;
}
}
NowTargetEntityInfoID = new long();
Settledlist = new List <long>();
getTarget = false;
var vector = mySensor.GetPosition() + (mySensor.WorldMatrix.Forward * 500);
mySensor.CustomData = vector.X + "," + vector.Y + "," + vector.Z;
return;
}
foreach (var item in detectedEntityInfos)
{
if (item.EntityId == NowTargetEntityInfoID)
{
mySensor.CustomData = item.Position.X + "," + item.Position.Y + "," + item.Position.Z;
return;
}
}
NowTargetEntityInfoID = new long();
Settledlist = new List <long>();
getTarget = false;
var vectorDi = mySensor.GetPosition() + (mySensor.WorldMatrix.Forward * 500);
mySensor.CustomData = vectorDi.X + "," + vectorDi.Y + "," + vectorDi.Z;
return;
}
}
double SensorDistance(Vector3D target, IMySensorBlock Sensor)
{
return(Vector3D.Distance(Sensor.GetPosition(), target));
}
