Space Engineers Smart Undock (WIP)

Script.cs

// Generated from ZerothAngel's SEScripts version 56430963671f tip
// Modules: undocktest, smartundock, gyrocontrol, thrustcontrol, shiporientation, shipcontrol, translateauto, pid, velocimeter, eventdriver, commons

// SmartUndock (ship-dependent)
const double SMART_UNDOCK_RTB_SPEED = 25.0; // In meters per second
const double AUTOPILOT_MIN_SPEED = 1.0; // In meters per second
const double AUTOPILOT_TTT_BUFFER = 5.0; // Time-to-target buffer, in seconds
const double AUTOPILOT_DISENGAGE_DISTANCE = 5.0; // In meters

// SmartUndock
const double SMART_UNDOCK_DISTANCE = 50.0; // In meters
const double SMART_UNDOCK_UNDOCK_SPEED = 5.0; // In meters per second

// ZACommons
const string STANDARD_LOOP_TIMER_BLOCK_NAME = "1 second loop";

private readonly EventDriver eventDriver = new EventDriver(timerName: STANDARD_LOOP_TIMER_BLOCK_NAME, timerGroup: "SmartUndockClock");
private readonly SmartUndock smartUndock = new SmartUndock();

private readonly ShipOrientation shipOrientation = new ShipOrientation();

private bool FirstRun = true;

void Main(string argument)
{
    var commons = new ShipControlCommons(this, shipOrientation);

    if (FirstRun)
    {
        FirstRun = false;

        shipOrientation.SetShipReference(commons, "SmartUndockReference");
    }

    smartUndock.HandleCommand(commons, eventDriver, argument);

    eventDriver.Tick(commons);
}

public class SmartUndock
{
    private readonly TranslateAutopilot autopilot = new TranslateAutopilot();

    private Vector3D? UndockTarget = null;

    public void HandleCommand(ZACommons commons, EventDriver eventDriver,
                              string argument)
    {
        argument = argument.Trim().ToLower();
        if (argument == "smartundock")
        {
            // First, determine which connector we were connected through
            IMyShipConnector connected = null;
            var connectors = ZACommons.GetBlocksOfType<IMyShipConnector>(commons.Blocks,
                                                                         connector => connector.DefinitionDisplayNameText == "Connector"); // Avoid Ejectors
            for (var e = connectors.GetEnumerator(); e.MoveNext();)
            {
                var connector = e.Current;
                if (connector.IsLocked && connector.IsConnected)
                {
                    // Assume the first one as well
                    connected = connector;
                    break;
                }
            }

            UndockTarget = null;
            if (connected != null)
            {
                // Undock the opposite direction of connector
                var forward = connected.Orientation.TransformDirection(Base6Directions.Direction.Backward);
                var up = connected.Orientation.TransformDirection(Base6Directions.Direction.Up);

                var reference = commons.Me;
                var backwardPoint = reference.CubeGrid.GridIntegerToWorld(reference.Position + Base6Directions.GetIntVector(forward));
                var backwardVector = Vector3D.Normalize(backwardPoint - reference.GetPosition());
                // Determine target undock point
                UndockTarget = reference.GetPosition() + SMART_UNDOCK_DISTANCE * backwardVector;

                // Schedule the autopilot
                autopilot.Init(commons, eventDriver, (Vector3D)UndockTarget,
                               SMART_UNDOCK_UNDOCK_SPEED,
                               delay: 2.0);
            }

            // Next, physically undock
            ZACommons.EnableBlocks(connectors, false);
            // Unlock landing gears as well
            var gears = ZACommons.GetBlocksOfType<IMyLandingGear>(commons.Blocks);
            gears.ForEach(gear =>
                    {
                        if (gear.IsLocked) gear.GetActionWithName("Unlock").Apply(gear);
                    });
        }
        else if (argument == "rtb")
        {
            // No target, no RTB
            if (UndockTarget == null) return;

            var shipControl = (ShipControlCommons)commons;

            // Schedule the autopilot
            autopilot.Init(commons, eventDriver, (Vector3D)UndockTarget,
                           SMART_UNDOCK_RTB_SPEED);
        }
        else if (argument == "smartreset")
        {
            autopilot.Reset(commons);
        }
    }
}

public class GyroControl
{
    public const int Yaw = 0;
    public const int Pitch = 1;
    public const int Roll = 2;

    public readonly string[] AxisNames = new string[] { "Yaw", "Pitch", "Roll" };

    public struct GyroAxisDetails
    {
        public int LocalAxis;
        public int Sign;

        public GyroAxisDetails(int localAxis, int sign)
        {
            LocalAxis = localAxis;
            Sign = sign;
        }
    }

    public struct GyroDetails
    {
        public IMyGyro Gyro;
        public GyroAxisDetails[] AxisDetails;

        public GyroDetails(IMyGyro gyro, Base6Directions.Direction shipUp,
                           Base6Directions.Direction shipForward)
        {
            Gyro = gyro;
            AxisDetails = new GyroAxisDetails[3];

            var shipLeft = Base6Directions.GetLeft(shipUp, shipForward);

            // Determine yaw axis
            switch (gyro.Orientation.TransformDirectionInverse(shipUp))
            {
                case Base6Directions.Direction.Up:
                    AxisDetails[Yaw] = new GyroAxisDetails(Yaw, -1);
                    break;
                case Base6Directions.Direction.Down:
                    AxisDetails[Yaw] = new GyroAxisDetails(Yaw, 1);
                    break;
                case Base6Directions.Direction.Left:
                    AxisDetails[Yaw] = new GyroAxisDetails(Pitch, 1);
                    break;
                case Base6Directions.Direction.Right:
                    AxisDetails[Yaw] = new GyroAxisDetails(Pitch, -1);
                    break;
                case Base6Directions.Direction.Forward:
                    AxisDetails[Yaw] = new GyroAxisDetails(Roll, 1);
                    break;
                case Base6Directions.Direction.Backward:
                    AxisDetails[Yaw] = new GyroAxisDetails(Roll, -1);
                    break;
            }

            // Determine pitch axis
            switch (gyro.Orientation.TransformDirectionInverse(shipLeft))
            {
                case Base6Directions.Direction.Up:
                    AxisDetails[Pitch] = new GyroAxisDetails(Yaw, -1);
                    break;
                case Base6Directions.Direction.Down:
                    AxisDetails[Pitch] = new GyroAxisDetails(Yaw, 1);
                    break;
                case Base6Directions.Direction.Left:
                    AxisDetails[Pitch] = new GyroAxisDetails(Pitch, -1);
                    break;
                case Base6Directions.Direction.Right:
                    AxisDetails[Pitch] = new GyroAxisDetails(Pitch, 1);
                    break;
                case Base6Directions.Direction.Forward:
                    AxisDetails[Pitch] = new GyroAxisDetails(Roll, 1);
                    break;
                case Base6Directions.Direction.Backward:
                    AxisDetails[Pitch] = new GyroAxisDetails(Roll, -1);
                    break;
            }

            // Determine roll axis
            switch (gyro.Orientation.TransformDirectionInverse(shipForward))
            {
                case Base6Directions.Direction.Up:
                    AxisDetails[Roll] = new GyroAxisDetails(Yaw, -1);
                    break;
                case Base6Directions.Direction.Down:
                    AxisDetails[Roll] = new GyroAxisDetails(Yaw, 1);
                    break;
                case Base6Directions.Direction.Left:
                    AxisDetails[Roll] = new GyroAxisDetails(Pitch, -1);
                    break;
                case Base6Directions.Direction.Right:
                    AxisDetails[Roll] = new GyroAxisDetails(Pitch, 1);
                    break;
                case Base6Directions.Direction.Forward:
                    AxisDetails[Roll] = new GyroAxisDetails(Roll, 1);
                    break;
                case Base6Directions.Direction.Backward:
                    AxisDetails[Roll] = new GyroAxisDetails(Roll, -1);
                    break;
            }
        }
    }

    private readonly List<GyroDetails> gyros = new List<GyroDetails>();

    public void Init(IEnumerable<IMyTerminalBlock> blocks,
                     Func<IMyGyro, bool> collect = null,
                     Base6Directions.Direction shipUp = Base6Directions.Direction.Up,
                     Base6Directions.Direction shipForward = Base6Directions.Direction.Forward)
    {
        gyros.Clear();
        for (var e = blocks.GetEnumerator(); e.MoveNext();)
        {
            var gyro = e.Current as IMyGyro;
            if (gyro != null &&
                gyro.IsFunctional && gyro.IsWorking && gyro.Enabled &&
                (collect == null || collect(gyro)))
            {
                var details = new GyroDetails(gyro, shipUp, shipForward);
                gyros.Add(details);
            }
        }
    }

    public void EnableOverride(bool enable)
    {
        gyros.ForEach(gyro => gyro.Gyro.SetValue<bool>("Override", enable));
    }

    public void SetAxisVelocity(int axis, float velocity)
    {
        gyros.ForEach(gyro => gyro.Gyro.SetValue<float>(AxisNames[gyro.AxisDetails[axis].LocalAxis], gyro.AxisDetails[axis].Sign * velocity));
    }

    public void SetAxisVelocityRPM(int axis, float rpmVelocity)
    {
        SetAxisVelocity(axis, rpmVelocity * MathHelper.RPMToRadiansPerSecond);
    }

    public void Reset()
    {
        gyros.ForEach(gyro => {
                gyro.Gyro.SetValue<float>("Yaw", 0.0f);
                gyro.Gyro.SetValue<float>("Pitch", 0.0f);
                gyro.Gyro.SetValue<float>("Roll", 0.0f);
            });
    }
}

public class ThrustControl
{
    private readonly Dictionary<Base6Directions.Direction, List<IMyThrust>> thrusters = new Dictionary<Base6Directions.Direction, List<IMyThrust>>();

    private void AddThruster(Base6Directions.Direction direction, IMyThrust thruster)
    {
        var thrusterList = GetThrusters(direction);
        thrusterList.Add(thruster);
    }

    public void Init(IEnumerable<IMyTerminalBlock> blocks,
                     Func<IMyThrust, bool> collect = null,
                     Base6Directions.Direction shipUp = Base6Directions.Direction.Up,
                     Base6Directions.Direction shipForward = Base6Directions.Direction.Forward)
    {
        MyBlockOrientation shipOrientation = new MyBlockOrientation(shipForward, shipUp);

        thrusters.Clear();
        for (var e = blocks.GetEnumerator(); e.MoveNext();)
        {
            var thruster = e.Current as IMyThrust;
            if (thruster != null && thruster.IsFunctional &&
                (collect == null || collect(thruster)))
            {
                var facing = thruster.Orientation.TransformDirection(Base6Directions.Direction.Forward); // Exhaust goes this way
                var thrustDirection = Base6Directions.GetFlippedDirection(facing);
                var shipDirection = shipOrientation.TransformDirectionInverse(thrustDirection);
                AddThruster(shipDirection, thruster);
            }
        }
    }

    public List<IMyThrust> GetThrusters(Base6Directions.Direction direction)
    {
        List<IMyThrust> thrusterList;
        if (!thrusters.TryGetValue(direction, out thrusterList))
        {
            thrusterList = new List<IMyThrust>();
            thrusters.Add(direction, thrusterList);
        }
        return thrusterList;
    }

    public void SetOverride(Base6Directions.Direction direction, bool enable = true)
    {
        var thrusterList = GetThrusters(direction);
        thrusterList.ForEach(thruster =>
                             thruster.SetValue<float>("Override", enable ?
                                                      thruster.GetMaximum<float>("Override") :
                                                      0.0f));
    }

    public void SetOverride(Base6Directions.Direction direction, double percent)
    {
        percent = Math.Max(percent, 0.0);
        percent = Math.Min(percent, 1.0);
        var thrusterList = GetThrusters(direction);
        thrusterList.ForEach(thruster =>
                             thruster.SetValue<float>("Override",
                                                      (float)(thruster.GetMaximum<float>("Override") * percent)));
    }

    public void SetOverrideNewtons(Base6Directions.Direction direction, double force)
    {
        var thrusterList = GetThrusters(direction);
        var maxForce = 0.0;
        thrusterList.ForEach(thruster =>
                             maxForce += thruster.GetMaximum<float>("Override"));
        // Constrain
        force = Math.Max(force, 0.0);
        force = Math.Min(force, maxForce);
        // Each thruster outputs its own share
        var fraction = force / maxForce;
        thrusterList.ForEach(thruster =>
                             thruster.SetValue<float>("Override",
                                                      (float)(fraction * thruster.GetMaximum<float>("Override"))));
    }

    public void Enable(Base6Directions.Direction direction, bool enable)
    {
        var thrusterList = GetThrusters(direction);
        thrusterList.ForEach(thruster => thruster.SetValue<bool>("OnOff", enable));
    }

    public void Enable(bool enable)
    {
        for (var e = thrusters.Values.GetEnumerator(); e.MoveNext();)
        {
            var thrusterList = e.Current;
            thrusterList.ForEach(thruster => thruster.SetValue<bool>("OnOff", enable));
        }
    }

    public void Reset()
    {
        for (var e = thrusters.Values.GetEnumerator(); e.MoveNext();)
        {
            var thrusterList = e.Current;
            thrusterList.ForEach(thruster => thruster.SetValue<float>("Override", 0.0f));
        }
    }
}

public class ShipOrientation
{
    public Base6Directions.Direction ShipUp { get; private set; }
    public Base6Directions.Direction ShipForward { get; private set; }

    public ShipOrientation()
    {
        // Defaults
        ShipUp = Base6Directions.Direction.Up;
        ShipForward = Base6Directions.Direction.Forward;
    }

    // Use orientation of given block
    public void SetShipReference(IMyCubeBlock reference)
    {
        ShipUp = reference.Orientation.TransformDirection(Base6Directions.Direction.Up);
        ShipForward = reference.Orientation.TransformDirection(Base6Directions.Direction.Forward);
    }

    // Use orientation of a block in the given group
    public void SetShipReference(ZACommons commons, string groupName,
                                 Func<IMyTerminalBlock, bool> condition = null)
    {
        var group = commons.GetBlockGroupWithName(groupName);
        if (group != null)
        {
            for (var e = group.Blocks.GetEnumerator(); e.MoveNext();)
            {
                var block = e.Current;
                if (block.CubeGrid == commons.Me.CubeGrid &&
                    (condition == null || condition(block)))
                {
                    SetShipReference(block);
                    return;
                }
            }
        }
        // Default to grid up/forward
        ShipUp = Base6Directions.Direction.Up;
        ShipForward = Base6Directions.Direction.Forward;
    }

    // Use orientation of a block of the given type
    public void SetShipReference<T>(IEnumerable<IMyTerminalBlock> blocks,
                                    Func<T, bool> condition = null)
        where T : IMyCubeBlock
    {
        var references = ZACommons.GetBlocksOfType<T>(blocks, condition);
        if (references.Count > 0)
        {
            SetShipReference(references[0]);
        }
        else
        {
            // Default to grid up/forward
            ShipUp = Base6Directions.Direction.Up;
            ShipForward = Base6Directions.Direction.Forward;
        }
    }
}

// Do not hold a reference to an instance of this class between runs
public class ShipControlCommons : ZACommons
{
    private readonly ShipOrientation shipOrientation;

    public Base6Directions.Direction ShipUp
    {
        get { return shipOrientation.ShipUp; }
    }

    public Base6Directions.Direction ShipForward
    {
        get { return shipOrientation.ShipForward; }
    }

    public ShipControlCommons(MyGridProgram program,
                              ShipOrientation shipOrientation,
                              string shipGroup = null)
        : base(program, shipGroup: shipGroup)
    {
        this.shipOrientation = shipOrientation;
    }

    public GyroControl GyroControl
    {
        get
        {
            if (m_gyroControl == null)
            {
                m_gyroControl = new GyroControl();
                m_gyroControl.Init(Blocks,
                                   shipUp: shipOrientation.ShipUp,
                                   shipForward: shipOrientation.ShipForward);
            }
            return m_gyroControl;
        }
    }
    private GyroControl m_gyroControl = null;

    public ThrustControl ThrustControl
    {
        get
        {
            if (m_thrustControl == null)
            {
                m_thrustControl = new ThrustControl();
                m_thrustControl.Init(Blocks,
                                     shipUp: shipOrientation.ShipUp,
                                     shipForward: shipOrientation.ShipForward);
            }
            return m_thrustControl;
        }
    }
    private ThrustControl m_thrustControl = null;
}

public class TranslateAutopilot
{
    public struct Orientation
    {
        public Vector3D Point;
        public Vector3D Forward;
        public Vector3D Up;
        public Vector3D Left;

        public Orientation(IMyCubeBlock reference,
                           Base6Directions.Direction shipUp = Base6Directions.Direction.Up,
                           Base6Directions.Direction shipForward = Base6Directions.Direction.Forward)
        {
            Point = reference.GetPosition();
            var forward3I = reference.Position + Base6Directions.GetIntVector(shipForward);
            Forward = Vector3D.Normalize(reference.CubeGrid.GridIntegerToWorld(forward3I) - Point);
            var up3I = reference.Position + Base6Directions.GetIntVector(shipUp);
            Up = Vector3D.Normalize(reference.CubeGrid.GridIntegerToWorld(up3I) - Point);
            var left3I = reference.Position + Base6Directions.GetIntVector(Base6Directions.GetLeft(shipUp, shipForward));
            Left = Vector3D.Normalize(reference.CubeGrid.GridIntegerToWorld(left3I) - Point);
        }
    }

    private const uint FramesPerRun = 2;
    private const double RunsPerSecond = 60.0 / FramesPerRun;

    private readonly Velocimeter velocimeter = new Velocimeter(30);
    private readonly PIDController forwardPID = new PIDController(1.0 / RunsPerSecond);
    private readonly PIDController upPID = new PIDController(1.0 / RunsPerSecond);
    private readonly PIDController leftPID = new PIDController(1.0 / RunsPerSecond);

    private const double ThrustKp = 1.0;
    private const double ThrustKi = 0.001;
    private const double ThrustKd = 1.0;

    private Vector3D AutopilotTarget;
    private double AutopilotSpeed;
    private bool AutopilotEngaged;

    public TranslateAutopilot()
    {
        forwardPID.Kp = ThrustKp;
        forwardPID.Ki = ThrustKi;
        forwardPID.Kd = ThrustKd;

        upPID.Kp = ThrustKp;
        upPID.Ki = ThrustKi;
        upPID.Kd = ThrustKd;

        leftPID.Kp = ThrustKp;
        leftPID.Ki = ThrustKi;
        leftPID.Kd = ThrustKd;
    }

    public void Init(ZACommons commons, EventDriver eventDriver,
                     Vector3D target, double speed,
                     double delay = 1.0)
    {
        if (!AutopilotEngaged)
        {
            AutopilotTarget = target;
            AutopilotSpeed = speed;
            AutopilotEngaged = true;
            eventDriver.Schedule(delay, Start);
        }
    }

    public void Start(ZACommons commons, EventDriver eventDriver)
    {
        var shipControl = (ShipControlCommons)commons;
        var gyroControl = shipControl.GyroControl;
        gyroControl.Reset();
        gyroControl.EnableOverride(true); // So the user knows it's engaged
        shipControl.ThrustControl.Reset();
        velocimeter.Reset();
        forwardPID.Reset();
        upPID.Reset();
        leftPID.Reset();
        eventDriver.Schedule(0, Run);
    }

    public void Run(ZACommons commons, EventDriver eventDriver)
    {
        if (!AutopilotEngaged)
        {
            Reset(commons);
            return;
        }

        var shipControl = (ShipControlCommons)commons;

        var reference = commons.Me;
        var orientation = new Orientation(reference,
                                          shipUp: shipControl.ShipUp,
                                          shipForward: shipControl.ShipForward);

        var targetVector = AutopilotTarget - orientation.Point;
        var distance = targetVector.Length();

        // Take projection of target vector on each of our axes
        var forwardError = Vector3D.Dot(targetVector, orientation.Forward);
        var upError = Vector3D.Dot(targetVector, orientation.Up);
        var leftError = Vector3D.Dot(targetVector, orientation.Left);

        velocimeter.TakeSample(reference.GetPosition(), eventDriver.TimeSinceStart);
        var velocity = velocimeter.GetAverageVelocity();
        if (velocity != null)
        {
            var forwardSpeed = Vector3D.Dot((Vector3D)velocity, orientation.Forward);
            var upSpeed = Vector3D.Dot((Vector3D)velocity, orientation.Up);
            var leftSpeed = Vector3D.Dot((Vector3D)velocity, orientation.Left);

            // Naive approach: independent control of each axis
            Thrust(shipControl, Base6Directions.Direction.Forward, forwardError, forwardSpeed, forwardPID);
            Thrust(shipControl, Base6Directions.Direction.Up, upError, upSpeed, upPID);
            Thrust(shipControl, Base6Directions.Direction.Left, leftError, leftSpeed, leftPID);
        }

        if (distance < AUTOPILOT_DISENGAGE_DISTANCE)
        {
            Reset(commons);
        }
        else
        {
            eventDriver.Schedule(FramesPerRun, Run);
        }
    }

    private void Thrust(ShipControlCommons commons, Base6Directions.Direction direction,
                        double distance, double speed, PIDController pid)
    {
        //commons.Echo(string.Format("Distance: {0:F1} m", distance));

        var thrustControl = commons.ThrustControl;
        var flipped = Base6Directions.GetFlippedDirection(direction);

        var targetSpeed = Math.Min(Math.Abs(distance) / AUTOPILOT_TTT_BUFFER,
                                   AutopilotSpeed);
        targetSpeed = Math.Max(targetSpeed, AUTOPILOT_MIN_SPEED); // Avoid Zeno's paradox...
        targetSpeed *= Math.Sign(distance);
        //commons.Echo(string.Format("Target Speed: {0:F1} m/s", targetSpeed));
        //commons.Echo(string.Format("Speed: {0:F1} m/s", speed));

        var error = targetSpeed - speed;
        var force = pid.Compute(error);

        if (Math.Abs(distance) < 1.0)
        {
            // Good enough
            thrustControl.SetOverride(direction, false);
            thrustControl.SetOverride(flipped, false);
        }
        else if (force > 0.0)
        {
            thrustControl.SetOverride(direction, force);
            thrustControl.SetOverride(flipped, false);
        }
        else
        {
            thrustControl.SetOverride(direction, false);
            thrustControl.SetOverride(flipped, -force);
        }
    }

    public void Reset(ZACommons commons)
    {
        var shipControl = (ShipControlCommons)commons;
        shipControl.GyroControl.EnableOverride(false);
        shipControl.ThrustControl.Reset();
        AutopilotEngaged = false;
    }
}

public class PIDController
{
    public readonly double dt; // i.e. 1.0 / ticks per second

    public double Kp { get; set; }

    public double Ki
    {
        get { return m_Ki; }
        set { m_Ki = value; m_Kidt = m_Ki * dt; }
    }
    private double m_Ki, m_Kidt;

    public double Kd
    {
        get { return m_Kd; }
        set { m_Kd = value; m_Kddt = m_Kd / dt; }
    }
    private double m_Kd, m_Kddt;

    private double integral = 0.0;
    private double lastError = 0.0;

    public PIDController(double dt)
    {
        this.dt = dt;
    }

    public void Reset()
    {
        integral = 0.0;
        lastError = 0.0;
    }

    public double Compute(double error)
    {
        integral += error;
        var derivative = error - lastError;
        lastError = error;

        return ((Kp * error) +
                (m_Kidt * integral) +
                (m_Kddt * derivative));
    }
}

public class Velocimeter
{
    public struct PositionSample
    {
        public Vector3D Position;
        public TimeSpan Timestamp;

        public PositionSample(Vector3D position, TimeSpan timestamp)
        {
            Position = position;
            Timestamp = timestamp;
        }
    }

    private readonly uint MaxSampleCount;
    private readonly LinkedList<PositionSample> Samples = new LinkedList<PositionSample>();

    public Velocimeter(uint maxSampleCount)
    {
        MaxSampleCount = maxSampleCount;
    }

    public void TakeSample(Vector3D position, TimeSpan timestamp)
    {
        var sample = new PositionSample(position, timestamp);
        Samples.AddLast(sample);
        // Age out old samples
        while (Samples.Count > MaxSampleCount)
        {
            Samples.RemoveFirst();
        }
    }

    public Vector3D? GetAverageVelocity()
    {
        // Need at least 2 samples...
        if (Samples.Count > 1)
        {
            var last = Samples.First;
            Vector3D distance = new Vector3D();
            double seconds = 0.0;
            for (var current = last.Next;
                 current != null;
                 current = current.Next)
            {
                distance += current.Value.Position - last.Value.Position;
                seconds += current.Value.Timestamp.TotalSeconds -
                    last.Value.Timestamp.TotalSeconds;
                last = current;
            }

            return distance / seconds;
        }
        return null;
    }

    public void Reset()
    {
        Samples.Clear();
    }
}

public class EventDriver
{
    public struct FutureTickAction : IComparable<FutureTickAction>
    {
        public ulong When;
        public Action<ZACommons, EventDriver> Action;

        public FutureTickAction(ulong when, Action<ZACommons, EventDriver> action = null)
        {
            When = when;
            Action = action;
        }

        public int CompareTo(FutureTickAction other)
        {
            return When.CompareTo(other.When);
        }
    }

    public struct FutureTimeAction : IComparable<FutureTimeAction>
    {
        public TimeSpan When;
        public Action<ZACommons, EventDriver> Action;

        public FutureTimeAction(TimeSpan when, Action<ZACommons, EventDriver> action = null)
        {
            When = when;
            Action = action;
        }

        public int CompareTo(FutureTimeAction other)
        {
            return When.CompareTo(other.When);
        }
    }

    private const ulong TicksPerSecond = 60;

    // Why is there no standard priority queue implementation?
    private readonly LinkedList<FutureTickAction> TickQueue = new LinkedList<FutureTickAction>();
    private readonly LinkedList<FutureTimeAction> TimeQueue = new LinkedList<FutureTimeAction>();
    private readonly string TimerName, TimerGroup;
    private ulong Ticks; // Not a reliable measure of time because of variable timer delay.

    public TimeSpan TimeSinceStart
    {
        get { return m_timeSinceStart; }
        private set { m_timeSinceStart = value; }
    }
    private TimeSpan m_timeSinceStart = TimeSpan.FromSeconds(0);

    // If neither timerName nor timerGroup are given, it's assumed the timer will kick itself
    public EventDriver(string timerName = null, string timerGroup = null)
    {
        TimerName = timerName;
        TimerGroup = timerGroup;
    }

    private void KickTimer(ZACommons commons)
    {
        IMyTimerBlock timer = null;
        // Name takes priority over group
        if (TimerName != null)
        {
            var blocks = ZACommons.SearchBlocksOfName(commons.Blocks, TimerName,
                                                      block => block is IMyTimerBlock);
            if (blocks.Count > 0)
            {
                timer = blocks[0] as IMyTimerBlock;
            }
        }
        if (timer == null && TimerGroup != null)
        {
            var group = commons.GetBlockGroupWithName(TimerGroup);
            if (group != null)
            {
                var blocks = ZACommons.GetBlocksOfType<IMyTimerBlock>(group.Blocks,
                                                                      block => block.CubeGrid == commons.Me.CubeGrid);
                timer = blocks.Count > 0 ? blocks[0] : null;
            }
        }

        if (timer != null)
        {
            // Rules are simple. If we have something in the tick queue, trigger now.
            // Otherwise, set timer delay appropriately (minimum 1 second) and kick.
            // If you want sub-second accuracy, always use ticks.
            if (TickQueue.First != null)
            {
                timer.GetActionWithName("TriggerNow").Apply(timer);
            }
            else if (TimeQueue.First != null)
            {
                var next = (float)(TimeQueue.First.Value.When.TotalSeconds - TimeSinceStart.TotalSeconds);
                // Constrain appropriately (not sure if this will be done for us or if it
                // will just throw). Just do it to be safe.
                next = Math.Max(next, timer.GetMininum<float>("TriggerDelay"));
                next = Math.Min(next, timer.GetMaximum<float>("TriggerDelay"));

                timer.SetValue<float>("TriggerDelay", next);
                timer.GetActionWithName("Start").Apply(timer);
            }
            // NB If both queues are empty, we stop running
        }
    }

    public void Tick(ZACommons commons, Action mainAction = null)
    {
        Ticks++;
        TimeSinceStart += commons.Program.ElapsedTime;

        bool runMain = false;

        // Process each queue independently
        while (TickQueue.First != null &&
               TickQueue.First.Value.When <= Ticks)
        {
            var action = TickQueue.First.Value.Action;
            TickQueue.RemoveFirst();
            if (action != null)
            {
                action(commons, this);
            }
            else
            {
                runMain = true;
            }
        }

        while (TimeQueue.First != null &&
               TimeQueue.First.Value.When <= TimeSinceStart)
        {
            var action = TimeQueue.First.Value.Action;
            TimeQueue.RemoveFirst();
            if (action != null)
            {
                action(commons, this);
            }
            else
            {
                runMain = true;
            }
        }

        if (runMain && mainAction != null) mainAction();

        KickTimer(commons);
    }

    public void Schedule(ulong delay, Action<ZACommons, EventDriver> action = null)
    {
        var future = new FutureTickAction(Ticks + delay, action);
        for (var current = TickQueue.First;
             current != null;
             current = current.Next)
        {
            if (future.CompareTo(current.Value) < 0)
            {
                // Insert before this one
                TickQueue.AddBefore(current, future);
                return;
            }
        }
        // Just add at the end
        TickQueue.AddLast(future);
    }

    public void Schedule(double seconds, Action<ZACommons, EventDriver> action = null)
    {
        var delay = Math.Max(seconds, 0.0);

        var future = new FutureTimeAction(TimeSinceStart + TimeSpan.FromSeconds(delay), action);
        for (var current = TimeQueue.First;
             current != null;
             current = current.Next)
        {
            if (future.CompareTo(current.Value) < 0)
            {
                // Insert before this one
                TimeQueue.AddBefore(current, future);
                return;
            }
        }
        // Just add at the end
        TimeQueue.AddLast(future);
    }
}

// Do NOT hold on to an instance of this class. Always instantiate a new one
// each run.
public class ZACommons
{
    public const StringComparison IGNORE_CASE = StringComparison.CurrentCultureIgnoreCase;

    public readonly MyGridProgram Program;
    private readonly string ShipGroupName;

    // All accessible blocks
    public List<IMyTerminalBlock> AllBlocks
    {
        get
        {
            // No multithreading makes lazy init so much easier
            if (m_allBlocks == null)
            {
                m_allBlocks = new List<IMyTerminalBlock>();
                Program.GridTerminalSystem.GetBlocks(m_allBlocks);
            }
            return m_allBlocks;
        }
    }
    private List<IMyTerminalBlock> m_allBlocks = null;

    // Blocks on the same grid only
    public List<IMyTerminalBlock> Blocks
    {
        get
        {
            if (m_blocks == null)
            {
                // Try group first, if it exists
                if (ShipGroupName != null)
                {
                    var group = GetBlockGroupWithName(ShipGroupName);
                    if (group != null) m_blocks = group.Blocks;
                }

                // Otherwise fetch all blocks on the same grid
                if (m_blocks == null)
                {
                    m_blocks = new List<IMyTerminalBlock>();
                    for (var e = AllBlocks.GetEnumerator(); e.MoveNext();)
                    {
                        var block = e.Current;
                        if (block.CubeGrid == Program.Me.CubeGrid) m_blocks.Add(block);
                    }
                }
            }
            return m_blocks;
        }
    }
    private List<IMyTerminalBlock> m_blocks = null;

    public List<IMyBlockGroup> Groups
    {
        get
        {
            if (m_groups == null)
            {
                m_groups = new List<IMyBlockGroup>();
                Program.GridTerminalSystem.GetBlockGroups(m_groups);
            }
            return m_groups;
        }
    }
    private List<IMyBlockGroup> m_groups = null;

    // NB Names are actually lowercased
    public Dictionary<string, IMyBlockGroup> GroupsByName
    {
        get
        {
            if (m_groupsByName == null)
            {
                m_groupsByName = new Dictionary<string, IMyBlockGroup>();
                for (var e = Groups.GetEnumerator(); e.MoveNext();)
                {
                    var group = e.Current;
                    m_groupsByName.Add(group.Name.ToLower(), group);
                }
            }
            return m_groupsByName;
        }
    }
    private Dictionary<string, IMyBlockGroup> m_groupsByName = null;

    public ZACommons(MyGridProgram program, string shipGroup = null)
    {
        Program = program;
        ShipGroupName = shipGroup;
    }

    // Groups

    public IMyBlockGroup GetBlockGroupWithName(string name)
    {
        IMyBlockGroup group;
        if (GroupsByName.TryGetValue(name.ToLower(), out group))
        {
            return group;
        }
        return null;
    }

    public List<IMyBlockGroup> GetBlockGroupsWithPrefix(string prefix)
    {
        var result = new List<IMyBlockGroup>();
        for (var e = Groups.GetEnumerator(); e.MoveNext();)
        {
            var group = e.Current;
            if (group.Name.StartsWith(prefix, IGNORE_CASE)) result.Add(group);
        }
        return result;
    }

    // Blocks

    public static List<T> GetBlocksOfType<T>(IEnumerable<IMyTerminalBlock> blocks,
                                             Func<T, bool> collect = null)
    {
        List<T> list = new List<T>();
        for (var e = blocks.GetEnumerator(); e.MoveNext();)
        {
            var block = e.Current;
            if (block is T && (collect == null || collect((T)block))) list.Add((T)block);
        }
        return list;
    }

    public static T GetBlockWithName<T>(IEnumerable<IMyTerminalBlock> blocks, string name)
        where T : IMyTerminalBlock
    {
        for (var e = blocks.GetEnumerator(); e.MoveNext();)
        {
            var block = e.Current;
            if(block is T && block.CustomName.Equals(name, IGNORE_CASE)) return (T)block;
        }
        return default(T);
    }

    public static List<IMyTerminalBlock> SearchBlocksOfName(IEnumerable<IMyTerminalBlock> blocks, string name, Func<IMyTerminalBlock, bool> collect = null)
    {
        var result = new List<IMyTerminalBlock>();
        for (var e = blocks.GetEnumerator(); e.MoveNext();)
        {
            var block = e.Current;
            if (block.CustomName.IndexOf(name, IGNORE_CASE) >= 0 &&
                (collect == null || collect(block)))
            {
                result.Add(block);
            }
        }
        return result;
    }

    public static void ForEachBlockOfType<T>(IEnumerable<IMyTerminalBlock> blocks, Action<T> action)
    {
        for (var e = blocks.GetEnumerator(); e.MoveNext();)
        {
            var block = e.Current;
            if (block is T)
            {
                action((T)block);
            }
        }
    }

    public static void EnableBlocks(IEnumerable<IMyTerminalBlock> blocks, bool enabled)
    {
        for (var e = blocks.GetEnumerator(); e.MoveNext();)
        {
            var block = e.Current;
            // Not all blocks will implement IMyFunctionalBlock, so can't checked Enabled
            block.GetActionWithName(enabled ? "OnOff_On" : "OnOff_Off").Apply(block);
        }
    }

    public IMyProgrammableBlock Me
    {
        get { return Program.Me; }
    }

    // Batteries

    public static bool IsBatteryRecharging(IMyBatteryBlock battery)
    {
        return !battery.ProductionEnabled;
    }

    public static void SetBatteryRecharge(IMyBatteryBlock battery, bool recharge)
    {
        var recharging = IsBatteryRecharging(battery);
        if ((recharging && !recharge) || (!recharging && recharge))
        {
            battery.GetActionWithName("Recharge").Apply(battery);
        }
    }

    public static void SetBatteryRecharge(IEnumerable<IMyBatteryBlock> batteries, bool recharge)
    {
        for (var e = batteries.GetEnumerator(); e.MoveNext();)
        {
            var battery = e.Current;
            SetBatteryRecharge(battery, recharge);
        }
    }

    // Display

    public Action<string> Echo
    {
        get { return Program.Echo; }
    }

    public static string FormatPower(float value)
    {
        if (value >= 1.0f)
        {
            return string.Format("{0:F2} MW", value);
        }
        else if (value >= 0.001)
        {
            return string.Format("{0:F2} kW", value * 1000f);
        }
        else
        {
            return string.Format("{0:F2} W", value * 1000000f);
        }
    }

    // Misc

    public static bool StartTimerBlockWithName(IEnumerable<IMyTerminalBlock> blocks, string name,
                                               Func<IMyTimerBlock, bool> condition = null)
    {
        var timer = GetBlockWithName<IMyTimerBlock>(blocks, name);
        if (timer != null && timer.Enabled && !timer.IsCountingDown &&
            (condition == null || condition(timer)))
        {
            timer.GetActionWithName("Start").Apply(timer);
            return true;
        }
        return false;
    }

    public static bool IsConnectedAnywhere(IEnumerable<IMyShipConnector> connectors)
    {
        for (var e = connectors.GetEnumerator(); e.MoveNext();)
        {
            var connector = e.Current;
            if (connector.IsLocked && connector.IsConnected)
            {
                return true;
            }
        }
        return false;
    }

    public static bool IsConnectedAnywhere(IEnumerable<IMyTerminalBlock> blocks)
    {
        return IsConnectedAnywhere(GetBlocksOfType<IMyShipConnector>(blocks));
    }
}

Set up: Programmable block & timer block. Timer block should have a single action only: run programmable block.

Timer block should either have "1 second loop" in its name or be part of a group named "SmartUndockClock"

Programmable block takes two arguments:

smartundock - Will disconnect from mothership and boost 50m away from connector

rtb - Will return ship to previous undock point. Will make a beeline, so be careful!