// 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)); }