public IEnumerator <bool> Init()
{
iniSerializer = new INISerializer("DroneControl");
iniSerializer.AddValue("controllerName", x => x, "Controller");
if (Me.CustomData == "")
{
string temp = Me.CustomData;
iniSerializer.FirstSerialization(ref temp);
Me.CustomData = temp;
}
else
{
iniSerializer.DeSerialize(Me.CustomData);
}
yield return(true);
time = new IngameTime();
GTS = new GridTerminalSystemUtils(Me, GridTerminalSystem);
yield return(true);
controller = GTS.GetBlockWithNameOnGrid(controllerName) as IMyShipController;
yield return(true);
entityTracking = new EntityTracking_Module(GTS, controller, null, EntityTracking_Module.refExpSettings.Turret);
autoPilot = new AutoPilot(GTS, controller, time, entityTracking);
yield return(true);
comms = new InterGridComms(IGC, time, Me);
comms.P = this;
yield return(true);
RegisterCommands();
loaded = true;
}
public override bool Update(AutoPilot owner, ref Vector3D linearV, ref Vector3D angularV)
{
if (Goal == null)
{
return(false);
}
IMyCubeBlock reference = Reference ?? owner.Controller;
MatrixD wm = reference.WorldMatrix;
Goal.UpdateTime(owner.elapsedTime);
Vector3D direction = Goal.CurrentPosition - wm.Translation;
double distance = direction.Normalize();
//linear velocity
linearV = direction * MaxLinearSpeed + Goal.Velocity;
//angular velocity
double diff = owner.RotateToMatch(direction, Vector3D.Zero,
wm.GetDirectionVector(ReferenceForward),
wm.GetDirectionVector(ReferenceUp),
ref angularV);
if (diff < OrientationEpsilon)
{
angularV = Vector3D.Zero;
}
return((diff < OrientationEpsilon) && (distance < PositionEpsilon));
}
/// <summary>
/// Queries the strategy on which linear and angular velocities the ship should have.
/// </summary>
/// <param name="owner">AutoPilot instance that queries the strategy.</param>
/// <param name="linearV">Initial value - current linear velocity. Is set to desired linear velocity.</param>
/// <param name="angularV">Initial value - current rotation. Is set to desired rotation.</param>
/// <returns>True if goal is considered achieved.</returns>
public override bool Update(AutoPilot owner, ref Vector3D linearV, ref Vector3D angularV)
{
if (Goal == null)
{
return(false);
}
IMyCubeBlock reference = Reference ?? owner.Controller;
Vector3D pos = Goal.CurrentPosition;
MatrixD wm = reference.WorldMatrix;
Vector3D direction = pos - wm.Translation;
double distance = direction.Normalize();
linearV.X = linearV.Y = linearV.Z = 0;
double diff = owner.RotateToMatch(direction, Vector3D.Zero,
wm.GetDirectionVector(ReferenceForward),
wm.GetDirectionVector(ReferenceUp),
ref angularV);
if (diff < OrientationEpsilon)
{
angularV = Vector3D.Zero;
return(true);
}
else
{
return(false);
}
}
/// <summary>
/// Queries the strategy on which linear and angular velocities the ship should have.
/// </summary>
/// <param name="owner">AutoPilot instance that queries the strategy.</param>
/// <param name="linearV">Initial value - current linear velocity. Is set to desired linear velocity.</param>
/// <param name="angularV">Initial value - current rotation. Is set to desired rotation.</param>
/// <returns>True if goal is considered achieved.</returns>
public override bool Update(AutoPilot owner, ref Vector3D linearV, ref Vector3D angularV)
{
if (Goal == null)
{
return(false);
}
bool distanceok = false;
bool orientationok = false;
IMyCubeBlock reference = Reference ?? owner.Controller;
MatrixD wm = reference.WorldMatrix;
Goal.UpdateTime(owner.elapsedTime);
Vector3D direction = Goal.CurrentPosition - wm.Translation;
double distance = direction.Normalize();
Vector3D facingdirection = direction; //we should face our goal, still.
if (distance < Goal.TargetDistance) //Are we too close to the goal?
{ // yep! better back off.
direction *= -1;
distance = Goal.TargetDistance - distance;
}
else //nah, we aren't there yet - just cut the distance we need to travel.
{
distance -= Goal.TargetDistance;
}
if (distance > PositionEpsilon) //Are we too far from our desired position?
{
//rotate the ship to face it
double diff = owner.RotateToMatch(facingdirection, Vector3D.Zero,
wm.GetDirectionVector(ReferenceForward),
wm.GetDirectionVector(ReferenceUp),
ref angularV);
if (diff > OrientationEpsilon) //we still need to rotate
{
linearV = Goal.Velocity; //match velocities with our target, then.
}
else //we are good
{
orientationok = true;
//how quickly can we go, assuming we still need to stop at the end?
double accel = owner.GetMaxAccelerationFor(-direction);
double braking_time = Math.Sqrt(2 * distance / accel);
double acceptable_velocity = Math.Min(VelocityUsage * accel * braking_time, MaxLinearSpeed);
//extra slowdown when close to the target
acceptable_velocity = Math.Min(acceptable_velocity, distance);
//moving relative to the target
linearV = direction * acceptable_velocity + Goal.Velocity;
angularV = Vector3D.Zero;
}
}
else //we are close to our ideal position - attempting to rotate the ship is not a good idea.
{
distanceok = true;
orientationok = true;
linearV = Goal.Velocity;
angularV = Vector3D.Zero;
}
return(distanceok && orientationok);
}
public override bool Update(AutoPilot owner, ref Vector3D linearV, ref Vector3D angularV)
{
if (Goal == null)
{
return(false);
}
IMyCubeBlock reference = Reference ?? owner.Controller;
MatrixD wm = reference.WorldMatrix;
Goal.UpdateTime(owner.elapsedTime);
Vector3D currentGoalPos = Goal.CurrentPosition;
Vector3D direction = currentGoalPos - wm.Translation;
double distance = direction.Normalize();
double target_distance = distance;
double diff;
if (!Vector3D.IsZero(Approach))
{
Vector3D minusApproach = -Approach;
diff = owner.RotateToMatch(Approach, Facing,
wm.GetDirectionVector(ReferenceForward),
wm.GetDirectionVector(ReferenceUp),
ref angularV);
PlaneD alignment = new PlaneD(wm.Translation, minusApproach);
Vector3D alignedPos = alignment.Intersection(ref currentGoalPos, ref minusApproach);
Vector3D correction = alignedPos - wm.Translation;
if (!Vector3D.IsZero(correction, PositionEpsilon)) //are we on approach vector?
{ //no - let's move there
direction = correction;
distance = direction.Normalize();
}
//otherwise, we can keep our current direction
}
else
{
diff = owner.RotateToMatch(direction, Facing,
wm.GetDirectionVector(ReferenceForward),
wm.GetDirectionVector(ReferenceUp),
ref angularV);
}
//rotate the ship to face it
if (diff > OrientationEpsilon) //we still need to rotate
{
linearV = Goal.Velocity; //match velocities with our target, then.
}
else //we are good
{
//how quickly can we go, assuming we still need to stop at the end?
double accel = owner.GetMaxAccelerationFor(-direction);
double braking_time = Math.Sqrt(2 * distance / accel);
double acceptable_velocity = Math.Min(VelocityUsage * accel * braking_time, MaxLinearSpeed);
//extra slowdown when close to the target
acceptable_velocity = Math.Min(acceptable_velocity, distance);
//moving relative to the target
linearV = direction * acceptable_velocity + Goal.Velocity;
angularV = Vector3D.Zero;
}
return(TryLockIn(distance) || (target_distance < PositionEpsilon));
}
private bool ScanForObstacles(AutoPilot owner, double distance)
{
foreach (IMyCameraBlock cam in Cameras)
{
if (cam.EnableRaycast)
{
Vector3D scan = cam.GetPosition() + TargetVector * distance;
cam.Enabled = true;
if (cam.CanScan(scan))
{
//owner.Log?.Invoke("Scanning via " + cam.CustomName);
Obstacle = cam.Raycast(scan);
return(!Obstacle.IsEmpty());
}
}
}
//owner.Log?.Invoke("No cameras can scan at this time.");
return(false);
}
/// <summary>
/// Queries the strategy on which linear and angular velocities the ship should have.
/// </summary>
/// <param name="owner">AutoPilot instance that queries the strategy.</param>
/// <param name="linearV">Initial value - current linear velocity. Is set to desired linear velocity.</param>
/// <param name="angularV">Initial value - current rotation. Is set to desired rotation.</param>
/// <returns>False, since orbiter has no goal to achieve.</returns>
public override bool Update(AutoPilot owner, ref Vector3D linearV, ref Vector3D angularV)
{
if (Goal == null)
{
return(false);
}
IMyCubeBlock reference = Reference ?? owner.Controller;
MatrixD wm = reference.WorldMatrix;
Vector3D radius = Goal.CurrentPosition - wm.Translation;
double R = radius.Normalize();
Vector3D vel = Normal.Cross(radius);
vel.Normalize();
linearV = Goal.Velocity + vel * MaxLinearSpeed + radius * (R - Goal.TargetDistance);
double diff = owner.RotateToMatch(radius, Normal,
wm.GetDirectionVector(ReferenceForward),
wm.GetDirectionVector(ReferenceUp),
ref angularV);
return(false);
}
/// <summary>
/// Queries the strategy on which linear and angular velocities the ship should have.
/// </summary>
/// <param name="owner">AutoPilot instance that queries the strategy.</param>
/// <param name="linearV">Initial value - current linear velocity. Is set to desired linear velocity.</param>
/// <param name="angularV">Initial value - current rotation. Is set to desired rotation.</param>
/// <returns>True if goal is considered achieved.</returns>
public override bool Update(AutoPilot owner, ref Vector3D linearV, ref Vector3D angularV)
{
if (Goal == null)
{
return(false);
}
IMyCubeBlock reference = Reference ?? owner.Controller;
MatrixD wm = reference.WorldMatrix;
Goal.UpdateTime(owner.elapsedTime);
Vector3D direction = Goal.CurrentPosition - wm.Translation;
double distance = direction.Normalize();
if (distance < Goal.TargetDistance)
{
direction *= -1;
distance = Goal.TargetDistance - distance;
}
if (distance > PositionEpsilon)
{
//linear velocity
double accel = owner.GetMaxAccelerationFor(-direction);
double braking_time = Math.Sqrt(2 * distance / accel);
double acceptable_velocity = Math.Min(VelocityUsage * accel * braking_time, MaxLinearSpeed);
acceptable_velocity = Math.Min(acceptable_velocity, distance);//slow down when close
Vector3D targetv = direction * acceptable_velocity;
linearV = targetv + Goal.Velocity;
}
else
{
linearV = Vector3D.Zero;
}
angularV = Vector3D.Zero;
if (distance < PositionEpsilon)
{
owner.Log?.Invoke("Target position reached.");
}
return(Vector3D.IsZero(linearV));
}
public override bool Update(AutoPilot owner, ref Vector3D linearV, ref Vector3D angularV)
{
throw new Exception();
}
public Program()
{
GridTerminalSystem.GetBlocksOfType <IMyTerminalBlock>(null, (b) =>
{
if (b.CubeGrid.EntityId == Me.CubeGrid.EntityId)
{
if (b is IMyThrust)
{
Thrusters.Add(b as IMyThrust);
}
else if (b is IMyGyro)
{
Gyros.Add(b as IMyGyro);
}
else if (b is IMySensorBlock)
{
Sensors.Add(b as IMySensorBlock);
}
else if (b is IMyBatteryBlock)
{
Batteries.Add(b as IMyBatteryBlock);
}
else if (b is IMyShipController)
{
Controller = b as IMyShipController;
}
else if (b is IMyShipConnector)
{
Connector = b as IMyShipConnector;
}
else if (b is IMyRadioAntenna)
{
Antenna = b as IMyRadioAntenna;
}
else if (b is IMyLandingGear)
{
Clamp = b as IMyLandingGear;
}
else if (b is IMyCameraBlock)
{
Cameras.Add(b as IMyCameraBlock);
}
}
return(false);
});
if (Controller == null)
{
throw new Exception("Controller not found.");
}
if (Thrusters.Count == 0)
{
throw new Exception("Thrusters not found.");
}
if (Gyros.Count == 0)
{
throw new Exception("Gyros not found.");
}
if (Connector == null)
{
throw new Exception("Connector not found.");
}
if (Clamp == null)
{
throw new Exception("Clamp not found.");
}
if (Sensors.Count == 0)
{
throw new Exception("Sensors not found.");
}
foreach (var cam in Cameras)
{
cam.EnableRaycast = true;
}
Pilot = new AutoPilot(Controller, Thrusters, Gyros);
try
{
Configure(Storage, true);
Message("Retriever Drone state restored.");
}
catch (Exception)
{
Configure(Me.CustomData, false);
Message("Retriever Drone initialized.");
}
Message($"Drone {Me.CubeGrid.CustomName} IGC ID: 0x{IGC.Me:X}");
logScreen = Me.GetSurface(0);
logScreen.ContentType = ContentType.TEXT_AND_IMAGE;
//Pilot.Log = (s) => logScreen?.WriteText(s, true);
}
public override bool Update(AutoPilot owner, ref Vector3D linearV, ref Vector3D angularV)
{
if (CurrentState == State.Destination)
{
return(true);
}
angularV = Vector3D.Zero;
IMyCubeBlock reference = Reference ?? owner.Controller;
MatrixD wm = reference.WorldMatrix;
TargetVector = wm.GetDirectionVector(ReferenceForward);
BoundingSphereD bs = reference.CubeGrid.WorldVolume;
Vector3D refpoint = bs.Center + TargetVector * (bs.Radius + Distance);
RaycastChargeSpeed = 0;
foreach (IMyCameraBlock cam in Cameras)
{
if (cam.IsWorking && (cam.WorldMatrix.Forward.Dot(TargetVector) > Math.Cos(cam.RaycastConeLimit)))
{
RaycastChargeSpeed += 2000.0;
cam.Enabled = true;
cam.EnableRaycast = true;
}
}
if (RaycastChargeSpeed == 0) //we can't scan in that orientation (or at all)!
{
throw new Exception("Can't scan!");
}
//owner.Log?.Invoke($"Direction: {TargetVector.X:F1}:{TargetVector.Y:F1}:{TargetVector.Z:F1}");
double accel = owner.GetMaxAccelerationFor(TargetVector);
double decel = owner.GetMaxAccelerationFor(-TargetVector);
//owner.Log?.Invoke($"Accel: {accel:F1}m/s^2");
//owner.Log?.Invoke($"Decel: {decel:F1}m/s^2");
if (decel == 0) //we can't decelerate afterwards!
{
throw new Exception("Can't decelerate!");
}
double speed = linearV.Length();
//account for possible acceleration
speed += accel * owner.elapsedTime;
//account for obstacle velocity relative to us
double obstacle_speed = Obstacle.IsEmpty() ? 0.0
: Math.Max(0, Obstacle.Velocity.Dot(-TargetVector));
speed += obstacle_speed;
//time required to charge the camera enough to scan
double scan_charge_time = (speed * speed) / (2 * decel * (RaycastChargeSpeed - speed));
//this is reaction time - how much time will pass before we can re-scan
scan_charge_time = Math.Max(scan_charge_time, owner.elapsedTime);
//distance requried to decelerate + distance we will pass in time till the next scan
double scan_dist = scan_charge_time * RaycastChargeSpeed;
scan_dist = Math.Max(scan_dist, 100);
if (ScanForObstacles(owner, scan_dist))
{ //obstacle detected - we are done
linearV = Vector3D.Zero;
return(true);
}
else
{ //no obstacles detected - fly at top speed
//owner.Log?.Invoke("Miss");
linearV = TargetVector * MaxLinearSpeed;
return(false);
}
}
public Program()
{
LogScreen = Me.GetSurface(0);
if (LogScreen != null)
{
LogScreen.ContentType = ContentType.TEXT_AND_IMAGE;
}
SavedState.TryParse(Storage);
Pilot = new AutoPilot(GridTerminalSystem, Me);
GridTerminalSystem.GetBlocksOfType <IMyTerminalBlock>(null, (b) =>
{
if (b.CubeGrid != Me.CubeGrid)
{
return(false);
}
if (b is IMyCockpit && b.IsFunctional)
{
Cockpit = b as IMyCockpit;
}
if ((b is IMyCameraBlock) &&
b.IsFunctional &&
(Pilot.Controller.WorldMatrix.Forward.Dot(b.WorldMatrix.Forward) > 0.9))
{
Front = b as IMyCameraBlock;
}
else if (b.BlockDefinition.TypeIdString == "MyObjectBuilder_ShipConnector" &&
b.BlockDefinition.SubtypeName != "ConnectorSmall" &&
b.IsFunctional)
{
Connector = b as IMyShipConnector;
}
else if (b is IMyBatteryBlock)
{
Batteries.Add(b as IMyBatteryBlock);
}
if (b.InventoryCount > 0 &&
!(b is IMyConveyorSorter) &&
b.BlockDefinition.SubtypeName != "ConnectorSmall")
{
StorageBlocks.Add(b);
}
return(false);
});
if (Cockpit == null)
{
throw new Exception("Cockpit not found.");
}
if (Connector == null)
{
throw new Exception("Connector not found.");
}
if (Front == null)
{
throw new Exception("Front camera not found.");
}
StorageBlocks.Sort((a, b) => ((double)b.GetInventory(0).MaxVolume).CompareTo((double)a.GetInventory(0).MaxVolume));
Front.EnableRaycast = true;
Middle = Cockpit.GetSurface(0);
Left = Cockpit.GetSurface(1);
Right = Cockpit.GetSurface(2);
Middle.ContentType = ContentType.TEXT_AND_IMAGE;
Middle.Font = "Monospace";
Middle.FontSize = 1.5f;
Left.ContentType = ContentType.TEXT_AND_IMAGE;
Left.Font = "Debug";
Left.FontSize = 2.0f;
Right.ContentType = ContentType.TEXT_AND_IMAGE;
Right.Font = "Debug";
Right.FontSize = 2.0f;
Runtime.UpdateFrequency = UpdateFrequency.Update10;
}
/// <summary> /// Queries the strategy on which linear and angular velocities the ship should have. /// </summary> /// <param name="owner">AutoPilot instance that queries the strategy.</param> /// <param name="linearV">Initial value - current linear velocity. Set it to desired linear velocity.</param> /// <param name="angularV">Initial value - current rotation. Set it to desired rotation.</param> /// <returns>True if goal is considered achieved.</returns> public abstract bool Update(AutoPilot owner, ref Vector3D linearV, ref Vector3D angularV);