public void ProcessDockingRequest()
{
Drone.LogToLcd($"\nLogging: {DateTime.Now}");
MyIGCMessage message = this.Drone.NetworkService.GetBroadcastListenerForChannel(DockingRequestChannel).AcceptMessage();
if (message.Data == null)
{
Drone.LogToLcd($"\nNo Message");
}
IMyShipConnector dockingPort = Drone.Grid().GetBlockWithName("Docking Port 1") as IMyShipConnector;
if (dockingPort == null)
{
Drone.LogToLcd("\nDocking Port 1 not found.");
}
else
{
Vector3D approachPoint = Vector3D.Add(dockingPort.GetPosition(), Vector3D.Multiply(dockingPort.WorldMatrix.Forward, 50));
List <Vector3D> dockingPath = new List <Vector3D> {
approachPoint, dockingPort.GetPosition()
};
Drone.LogToLcd($"Sending message: {dockingPort.WorldMatrix.Forward},{dockingPath[0].ToString()},{dockingPath[1].ToString()}");
this.Drone.NetworkService.BroadcastMessage(
DockingRequestChannel,
$"{dockingPort.WorldMatrix.Forward},{dockingPath[0].ToString()},{dockingPath[1].ToString()}"
);
}
}
public void Main(string argument)
{
// The main entry point of the script, invoked every time
// one of the programmable block's Run actions are invoked.
//
// The method itself is required, but the argument above
// can be removed if not needed.
List <IMyTerminalBlock> blocklist = new List <IMyTerminalBlock>();
IMyTextPanel lcd = null;
IMyShipConnector connector = null;
IMyShipController rc = null;
Vector3D v = new Vector3D();
string s = "";
GridTerminalSystem.GetBlocksOfType <IMyTextPanel>(blocklist);
lcd = blocklist[0] as IMyTextPanel;
GridTerminalSystem.GetBlocksOfType <IMyShipController>(blocklist);
rc = blocklist[0] as IMyShipController;
GridTerminalSystem.GetBlocksOfType <IMyShipConnector>(blocklist);
connector = blocklist[0] as IMyShipConnector;
v = connector.GetPosition();
s += "x: " + Math.Round(v.X, 2).ToString() + ", ";
s += "y: " + Math.Round(v.Y, 2).ToString() + ", ";
s += "z: " + Math.Round(v.Z, 2).ToString() + ";\n";
v = connector.GetPosition() + 2.5 * connector.WorldMatrix.Forward;
s += "x: " + Math.Round(v.X, 2).ToString() + ", ";
s += "y: " + Math.Round(v.Y, 2).ToString() + ", ";
s += "z: " + Math.Round(v.Z, 2).ToString() + ";\n";
BoundingBoxD test;
test.
lcd.WritePublicText(s);
}
public void Main(string argument, UpdateType updateSource)
{
var grid_orient = _ground_dock.CubeGrid.GridIntegerToWorld(new Vector3I());
_debug.WriteLine($"Position: {_ground_dock.Position}");
MatrixD g2w = GetGrid2WorldTransform(_ground_dock.CubeGrid);
Vector3D gridPos = (new Vector3D(_ground_dock.Min + _ground_dock.Max)) / 2.0; //( .Position is a problem for even size blocks)
Vector3D calcPos = Vector3D.Transform(gridPos, ref g2w);
double err = (_ground_dock.GetPosition() - calcPos).Length();
//Find the world "forward" vector for the block
MatrixD b2w = GetBlock2WorldTransform(_ground_dock);
Vector3D fwd = b2w.Forward;
fwd.Normalize(); //(Need to normalize because the above matrices are scaled by grid size)
_debug.WriteLine($"{_ground_dock.CustomName}: Error={err:f3} fwd={fwd.X:f3},{fwd.Y:f3},{fwd.Z:f3}");
//_display.WriteText($"{grid_orient}");
}
/// <summary>Requests the autoconnector to disconnect</summary>
public void Disconnect()
{
this.waypoints.Clear();
IMyShipConnector connector = this.getCurrentConnected()
?? this.getConnector(this.lastConnectionType)
?? this.frontConnector
?? this.downConnector;
Vector3D curPos = this.transformer.Pos(connector.GetPosition());
Vector3D curOrientation = this.transformer.Dir(connector.WorldMatrix.Backward);
connector.Disconnect();
Vector3D safetyTarget = curPos + (curOrientation * CONNECTION_SAFETY_OFFSET);
this.waypoints.Enqueue(new Waypoint(
new Vector3D(safetyTarget.X, safetyTarget.Y, safetyTarget.Z),
this.stator?.Angle ?? 0,
needPrecision: true
)).Enqueue(new Waypoint(
new Vector3D(safetyTarget.X, this.max.Y, safetyTarget.Z)
)).Enqueue(new Waypoint(
this.restPosition
));
}
/// <summary>
/// This method will update the HomeLocation information about the station connector position and orientation.<br />
/// Requires the ship to be connected.
/// </summary>
/// <param name="my_connector"></param>
/// <param name="station_connector"></param>
public void UpdateData(IMyShipConnector my_connector, IMyShipConnector station_connector)
{
shipConnectorID = my_connector.EntityId;
shipConnector = my_connector;
stationConnectorID = station_connector.EntityId;
stationConnectorPosition = station_connector.GetPosition();
stationConnectorForward = station_connector.WorldMatrix.Forward;
stationConnectorLeft = station_connector.WorldMatrix.Left;
stationConnectorName = station_connector.CustomName;
stationGridName = station_connector.CubeGrid.CustomName;
var normalizedleft = Vector3D.Normalize(PID.ProjectPointOnPlane(stationConnectorForward, Vector3D.Zero,
my_connector.WorldMatrix.Left));
var saved_up = normalizedleft.Cross(stationConnectorForward);
stationConnectorUpGlobal = saved_up;
stationConnectorUpLocal = worldDirectionToLocalDirection(stationConnectorUpGlobal, station_connector.WorldMatrix);
stationGridID = station_connector.CubeGrid.EntityId;
stationConnectorSize = ShipSystemsAnalyzer.GetRadiusOfConnector(station_connector);
}
Vector3D dockPos3 = new Vector3D(); //Straight Up And Forward Location
private void DockingIterator(bool Docking, List <Vector3D> COORDINATES, IMyGyro GYRO, IMyShipConnector CONNECTOR, IMyRemoteControl RC)
{
if (COORDINATES.Count < 3)
{
return;
}
int TargetID = 0;
int CurrentID = 0;
int iter_er = 0;
if (Docking == true)
{
TargetID = 1; CurrentID = 0; iter_er = +1;
}
if (Docking == false)
{
TargetID = 0; CurrentID = 1; iter_er = -1;
}
if (Docking == true)
{
CONNECTOR.Connect();
}
if (Docking == true && CONNECTOR.IsWorking == false)
{
CONNECTOR.Enabled = true;
}
if (Docking == false && CONNECTOR.IsWorking == true)
{
CONNECTOR.Disconnect(); CONNECTOR.Enabled = true;
}
if (CONNECTOR.Status == MyShipConnectorStatus.Connected && Docking == true)
{
for (int j = 0; j < CAF2_THRUST.Count; j++)
{
(CAF2_THRUST[j] as IMyThrust).Enabled = false;
}
GYRO.GyroOverride = false;
return;
}
Vector3D RollOrienter = Vector3D.Normalize(COORDINATES[COORDINATES.Count - 1] - COORDINATES[COORDINATES.Count - 2]);
Vector3D Connector_Direction = -1 * ReturnConnectorDirection(CONNECTOR, RC);
double RollReqt = (float)(0.6 * (Vector3D.Dot(RollOrienter, Connector_Direction)));
//垂直运动在码头
if (COORD_ID == COORDINATES.Count - 1)
{
Vector3D DockingHeading = Vector3D.Normalize(COORDINATES[COORDINATES.Count - 3] - COORDINATES[COORDINATES.Count - 2]) * 9000000; //Heading
GyroTurn6(DockingHeading, RotationalSensitvity, GYRO, RC, RollReqt, PrecisionMaxAngularVel); //Turn to heading
if (Vector3D.Dot(RC.WorldMatrix.Forward, Vector3D.Normalize(DockingHeading)) > 0.98) //Error check for small rotational velocity
{
Vector_Thrust_Manager(COORDINATES[COORD_ID - TargetID], COORDINATES[COORD_ID - CurrentID], CONNECTOR.GetPosition(), 5, 0.7, RC);
} //Thrusts to point
}
//在码头上的最后/第一个外部Coord
else if (COORD_ID == 0)
{
print($"启动自动驾驶\n距离目标:{Vector3D.Distance(COORDINATES[0], RC.GetPosition())}");
RC_Manager(COORDINATES[0], RC, false);
}
//水平和迭代语句
else
{
var HEADING = Vector3D.Normalize(COORDINATES[COORD_ID - CurrentID] - COORDINATES[COORD_ID - TargetID]) * 9000000;
Vector_Thrust_Manager(COORDINATES[COORD_ID - TargetID], COORDINATES[COORD_ID - CurrentID], CONNECTOR.GetPosition(), 8, 1, RC); //Runs docking sequence
GyroTurn6(HEADING, RotationalSensitvity, GYRO, RC, RollReqt, PrecisionMaxAngularVel);
}
//逻辑检查和迭代
if (Docking == false && COORD_ID == 0)
{
}
else if ((CONNECTOR.GetPosition() - COORDINATES[COORD_ID - CurrentID]).Length() < 1 || ((RC.GetPosition() - COORDINATES[COORD_ID - CurrentID]).Length() < 10 && COORD_ID == 0))
{
COORD_ID = COORD_ID + iter_er;
if (COORD_ID == COORDINATES.Count)
{
COORD_ID = COORDINATES.Count - 1;
}
if (COORD_ID < 0)
{
COORD_ID = 0;
}
}
}
Boolean Dock3()
{
mode = 3;
foreach (IMyThrust thruster in allThrusters)
{
thruster.ThrustOverridePercentage = 0;
}
rc.SetAutoPilotEnabled(false);
foreach (IMyGyro gyro in gyros)
{
gyro.ApplyAction("OnOff_On");
}
double yaw;
double pitch;
Boolean yawLock = false;
Boolean pitchLock = false;
GetRotationAngles(Vector3D.Negate(dockingDir), connector, out yaw, out pitch);
ApplyGyroOverride(pitch, yaw, 0, gyros, connector);
if (yaw < 0.01)
{
yawLock = true;
}
if (pitch < 0.01)
{
pitchLock = true;
}
Echo("yawLock:" + yawLock);
Echo("pitchLock" + pitchLock);
if (pitchLock && yawLock)
{
Vector3D closestPoint;
double distanceFromDockingVector = DistanceFromVector(dockingConnectorPos, dockingDir, connector.GetPosition(), out closestPoint);
Echo("Distance from docking vector:" + distanceFromDockingVector);
if (distanceFromDockingVector > 0.35)
{
NewAdjustTest(closestPoint, connector, distanceFromDockingVector);
}
else if (distanceFromDockingVector == -1)
{
Echo("Error in docking vector distance calculation");
}
else
{
float shipmass = rc.CalculateShipMass().TotalMass;
if (rc.GetShipSpeed() < 1.5)
{
foreach (IMyThrust thruster in backThrusterGroup)
{
thruster.ThrustOverride = shipmass;
}
}
else
{
foreach (IMyThrust thruster in backThrusterGroup)
{
thruster.ThrustOverridePercentage = 0;
}
}
if (Vector3D.Distance(dockingConnectorPos, rc.GetPosition()) > errorDistance)
{
Storage = "";
dockingConnectorPos = new Vector3D();
dockingDir = new Vector3D();
mode = 0;
globalPath.Clear();
rc.ClearWaypoints();
foreach (IMyThrust thrust in allThrusters)
{
thrust.ThrustOverridePercentage = 0;
}
foreach (IMyGyro gyro in gyros)
{
gyro.GyroOverride = false;
gyro.Pitch = 0;
gyro.Yaw = 0;
gyro.Roll = 0;
}
Boolean sent = antenna.TransmitMessage("canceldock," + password);
if (!sent)
{
messageQue.Add("canceldock," + password);
}
antenna.CustomName += " ERROR";
}
}
}
if (connector.Status == MyShipConnectorStatus.Connectable)
{
connector.Connect(); mode = 5;
globalPath.Clear();
foreach (IMyThrust thruster in allThrusters)
{
thruster.ThrustOverridePercentage = 0;
}
foreach (IMyGyro gyro in gyros)
{
gyro.Pitch = 0;
gyro.Yaw = 0;
gyro.Roll = 0;
}
return(true);
}
return(false);
}
public override void HandleCallback(string callback)
{
switch (callback)
{
case "unicast":
MyIGCMessage message = Drone.NetworkService.GetUnicastListener().AcceptMessage();
if (message.Data == null)
{
Drone.LogToLcd($"\nNo Message");
}
if (message.Tag == DockingRequestChannel)
{
Drone.LogToLcd("\nReceived Docking Request");
IMyShipConnector dockingPort = GetFreeDockingPort();
if (dockingPort == null)
{
Drone.LogToLcd("\nNo Free Docking Port");
}
else
{
MyTuple <Vector3D, Vector3D, Vector3D> payload = new MyTuple <Vector3D, Vector3D, Vector3D>();
payload.Item1 = dockingPort.GetPosition() + dockingPort.WorldMatrix.Forward * 40;
payload.Item2 = dockingPort.GetPosition() + 1.5 * dockingPort.WorldMatrix.Forward;
Drone.LogToLcd($"\nClearance granted: {message.Source}");
Drone.LogToLcd($"\nApproach: {payload.Item1.ToString()}");
Drone.LogToLcd($"\nDocking Port: { payload.Item2.ToString()}");
Drone.Program.IGC.SendUnicastMessage(message.Source, DockingRequestChannel, payload);
}
}
else if (message.Tag == "Notifications")
{
Drone.LogToLcd($"Received notification:{message.Data.ToString()}");
DroneKlaxon.LoopPeriod = 2f;
DroneKlaxon.Play();
}
else if (message.Tag == "survey_reports")
{
MyTuple <long, string, double, Vector3D, Vector3D, Vector3D> report = (MyTuple <long, string, double, Vector3D, Vector3D, Vector3D>)message.Data;
Drone.LogToLcd($"Received survey_report: {report.ToString()}");
//TODO: This needs to be in a persistence layer, not the CustomData
MyIni ini = new MyIni();
MyIniParseResult config;
if (!ini.TryParse(Drone.Program.Me.CustomData, out config))
{
throw new Exception($"Error parsing config: {config.ToString()}");
}
//TODO: what about multiple deposits?
ini.Set($"deposit {report.Item1.ToString()}", "deposit_type", report.Item2.ToString());
ini.Set($"deposit {report.Item1.ToString()}", "deposit_depth", report.Item3.ToString());
ini.Set($"deposit {report.Item1.ToString()}", "top_left_corner", report.Item4.ToString());
ini.Set($"deposit {report.Item1.ToString()}", "top_right_corner", report.Item5.ToString());
ini.Set($"deposit {report.Item1.ToString()}", "bottom_left_corner", report.Item6.ToString());
ini.Set($"deposit {report.Item1.ToString()}", "index", 0);
Drone.Program.Me.CustomData = ini.ToString();
}
else if (message.Tag == "tunnel_complete")
{
MyIni ini = new MyIni();
MyIniParseResult config;
if (!ini.TryParse(Drone.Program.Me.CustomData, out config))
{
throw new Exception($"Error parsing config: {config.ToString()}");
}
int completedIndex = (int)message.Data;
List <string> sections = new List <string>();
ini.GetSections(sections);
IEnumerable <string> deposits = sections.Where(record => record.StartsWith("deposit"));
string deposit;
if (deposits != null && deposits.Count() != 0)
{
deposit = deposits.First();
}
else
{
throw new Exception("No deposit data found!");
}
ini.Set(deposit, "index", completedIndex + 1);
Drone.Program.Me.CustomData = ini.ToString();
}
break;
case "docking_request_pending":
ProcessDockingRequest();
break;
case "recall_miner":
Drone.LogToLcd("Recalling miner");
Drone.Program.IGC.SendUnicastMessage(MinerAddress, "recall", "recall");
break;
case "recall_surveyor":
Drone.LogToLcd("Recalling surveyor");
Drone.Program.IGC.SendUnicastMessage(SurveyorAddress, "recall", "recall");
break;
case "deploy_miner":
Drone.LogToLcd("Launching miner");
List <IMyProgrammableBlock> miners = new List <IMyProgrammableBlock>();
Drone.Grid().GetBlocksOfType(miners, pb => MyIni.HasSection(pb.CustomData, "miner"));
//We only support one miner for now
// Set the deposit details in the miners config
IMyProgrammableBlock miner = miners.First();
MyIni minerConfig = new MyIni();
MyIniParseResult result;
if (!minerConfig.TryParse(miner.CustomData, out result))
{
Drone.LogToLcd(miner.CustomData);
throw new Exception($"Error parsing config: {result.ToString()}");
}
//Vector3D MiningSite = DepositCentre + 10 * Vector3D.Normalize(DepositNormal);
//Vector3D TunnelEnd = DepositCentre - DepositDepth * Vector3D.Normalize(DepositNormal);
//get deposit data from catalogue
//calculate mining_site for next tunnel
//calculate tunnel_end from mining_site and depth
MyIni asteroidCatalogue = new MyIni();
asteroidCatalogue.TryParse(Drone.Program.Me.CustomData);
Tunnel tunnel = new Tunnel(asteroidCatalogue);
minerConfig.Set("miner", "mining_site", tunnel.StartingPoint.ToString());
minerConfig.Set("miner", "tunnel_end", tunnel.EndPoint.ToString());
minerConfig.Set("miner", "index", tunnel.TunnelIndex);
miner.CustomData = minerConfig.ToString();
miner.TryRun("launch");
break;
case "deploy_surveyor":
Drone.LogToLcd("Launching surveyor");
List <IMyProgrammableBlock> surveyors = new List <IMyProgrammableBlock>();
Drone.Grid().GetBlocksOfType(surveyors, pb => MyIni.HasSection(pb.CustomData, "surveyor"));
IMyProgrammableBlock surveyor = surveyors.First();
surveyor.TryRun("launch");
break;
case "echo":
Drone.LogToLcd("Echo");
DroneKlaxon.Play();
break;
case "":
// Just Ignore empty arguments
break;
default:
Drone.LogToLcd($"\nDrone received unrecognized callback: {callback}");
break;
}
}