PID Tester

// -Z is forward
// +X is right
// +Y is up
const Vector3D inputVec = new Vector3D(1,0,-1);
const double kP = 10;
const double kI = 0;
const double kD = 5;
const int runsPerSecond = 10;


// End of variables


int ticksPerRun = 60 / runsPerSecond;
int currentTicks = 141;

Vector3D targetVec;
bool align = false;

PID yawPID;
PID pitchPID;

Program()
{
    Runtime.UpdateFrequency = UpdateFrequency.Update1;
    yawPID = new PID(kP, kI, kD, 0.25, 1.0 / (double)runsPerSecond);
    pitchPID = new PID(kP, kI, kD, 0.25, 1.0 / (double)runsPerSecond);
}

void Main(string arg, UpdateType updateSource)
{
    var shipController = GetFirstBlockOfType<IMyShipController>();
    var gyros = new List<IMyGyro>();
    GridTerminalSystem.GetBlocksOfType(gyros, x => !x.CustomName.Contains("Ignore"));

    if (shipController == null || gyros.Count == 0)
    {
        Echo("ERROR: No ship controller and/or gyro");
        return;
    }

    switch (arg.ToLower().Trim())
    {
        case "start":
            targetVec = Vector3D.Rotate(inputVec, shipController.WorldMatrix);
            align = true;
            yawPID.Reset();
            pitchPID.Reset();
            break;
        case "stop":
            align = false;
            break;
    }

    currentTicks++;
    if (currentTicks < ticksPerRun)
        return;

    currentTicks = 0;


    if (!align)
    {
        ApplyGyroOverride(0, 0, 0, gyros, shipController);
        Echo("Not aligning");
    }
    else
    {
        double pitch = 0, yaw = 0;
        GetRotationAngles(targetVec, shipController.WorldMatrix, out yaw, out pitch);

        var yawSpeed = yawPID.Control(yaw);
        var pitchSpeed = pitchPID.Control(pitch);

        Echo($"Yaw: {yaw:n4}");
        Echo($"yawSpeed: {yawSpeed:n2}");
        Echo($"pitch: {pitch:n4}");
        Echo($"pitchSpeed: {pitchSpeed:n2}");

        ApplyGyroOverride(pitchSpeed, yawSpeed, 0, gyros, shipController);
        Echo("Aligning...");
    }
}

T GetFirstBlockOfType<T>() where T : class, IMyTerminalBlock
{
    var blocks = new List<T>();
    GridTerminalSystem.GetBlocksOfType(blocks);

    return blocks.Count > 0 ? blocks[0] : null;
}

//Whip's PID controller class v8 - 5/2/18
public class PID
{
    double _kP = 0;
    double _kI = 0;
    double _kD = 0;
    double _integralDecayRatio = 0;
    double _lowerBound = 0;
    double _upperBound = 0;
    double _timeStep = 0;
    double _inverseTimeStep = 0;
    double _errorSum = 0;
    double _lastError = 0;
    double _scalingThreshold = -1;
    bool _firstRun = true;
    bool _integralDecay = false;
    bool _scaleDerivative = false;
    public double Value { get; private set; }

    public PID(double kP, double kI, double kD, double lowerBound, double upperBound, double timeStep)
    {
        _kP = kP;
        _kI = kI;
        _kD = kD;
        _lowerBound = lowerBound;
        _upperBound = upperBound;
        _timeStep = timeStep;
        _inverseTimeStep = 1 / _timeStep;
        _integralDecay = false;
        _scaleDerivative = false;
    }

    public PID(double kP, double kI, double kD, double integralDecayRatio, double timeStep)
    {
        _kP = kP;
        _kI = kI;
        _kD = kD;
        _timeStep = timeStep;
        _inverseTimeStep = 1 / _timeStep;
        _integralDecayRatio = integralDecayRatio;
        _integralDecay = true;
        _scaleDerivative = false;
    }

    public PID(double kP, double kI, double kD, double integralDecayRatio, double timeStep, double scalingThreshold, bool placeholder)
    {
        _kP = kP;
        _kI = kI;
        _kD = kD;
        _timeStep = timeStep;
        _inverseTimeStep = 1 / _timeStep;
        _integralDecayRatio = integralDecayRatio;
        _scalingThreshold = scalingThreshold;
        _integralDecay = true;
        _scaleDerivative = true;
    }

    public double Control(double error)
    {
        //Compute derivative term
        var errorDerivative = (error - _lastError) * _inverseTimeStep;

        if (_firstRun)
        {
            errorDerivative = 0;
            _firstRun = false;
        }

        //Compute integral term
        if (!_integralDecay)
        {
            _errorSum += error * _timeStep;

            //Clamp integral term
            if (_errorSum > _upperBound)
                _errorSum = _upperBound;
            else if (_errorSum < _lowerBound)
                _errorSum = _lowerBound;
        }
        else
        {
            _errorSum = _errorSum * (1.0 - _integralDecayRatio) + error * _timeStep;
        }

        //Store this error as last error
        _lastError = error;

        //Construct output

        this.Value = _kP * error + _kI * _errorSum + (_scaleDerivative && Math.Abs(error) < _scalingThreshold ? Math.Abs(error / _scalingThreshold) * _kD * errorDerivative : _kD * errorDerivative);
        return this.Value;
    }

    public double Control(double error, double timeStep)
    {
        _timeStep = timeStep;
        _inverseTimeStep = 1 / _timeStep;
        return Control(error);
    }

    public void Reset()
    {
        _errorSum = 0;
        _lastError = 0;
        _firstRun = true;
    }
}

void GetRotationAngles(Vector3D targetVector, MatrixD worldMatrix, out double yaw, out double pitch)
{
    var localTargetVector = Vector3D.TransformNormal(targetVector, MatrixD.Transpose(worldMatrix));
    var flattenedTargetVector = new Vector3D(localTargetVector.X, 0, localTargetVector.Z);

    yaw = VectorAngleBetween(Vector3D.Forward, flattenedTargetVector) * Math.Sign(localTargetVector.X); //right is positive
    if (Math.Abs(yaw) < 1E-6 && localTargetVector.Z > 0) //check for straight back case
        yaw = Math.PI;

    if (Vector3D.IsZero(flattenedTargetVector)) //check for straight up case
        pitch = MathHelper.PiOver2 * Math.Sign(localTargetVector.Y);
    else
        pitch = VectorAngleBetween(localTargetVector, flattenedTargetVector) * Math.Sign(localTargetVector.Y); //up is positive
}

Vector3D VectorProjection(Vector3D a, Vector3D b)
{
    if (Vector3D.IsZero(b))
        return Vector3D.Zero;

    return a.Dot(b) / b.LengthSquared() * b;
}

double VectorAngleBetween(Vector3D a, Vector3D b) //returns radians
{
    if (Vector3D.IsZero(a) || Vector3D.IsZero(b))
        return 0;
    else
        return Math.Acos(MathHelper.Clamp(a.Dot(b) / Math.Sqrt(a.LengthSquared() * b.LengthSquared()), -1, 1));
}

//Whip's ApplyGyroOverride Method v9 - 8/19/17
void ApplyGyroOverride(double pitch_speed, double yaw_speed, double roll_speed, List<IMyGyro> gyro_list, IMyTerminalBlock reference)
{
    var rotationVec = new Vector3D(-pitch_speed, yaw_speed, roll_speed);

    var shipMatrix = reference.WorldMatrix;
    var relativeRotationVec = Vector3D.TransformNormal(rotationVec, shipMatrix);

    foreach (var thisGyro in gyro_list)
    {
        var gyroMatrix = thisGyro.WorldMatrix;
        var transformedRotationVec = Vector3D.TransformNormal(relativeRotationVec, Matrix.Transpose(gyroMatrix));

        thisGyro.Pitch = (float)transformedRotationVec.X; //because keen does some weird stuff with signs
        thisGyro.Yaw = (float)transformedRotationVec.Y;
        thisGyro.Roll = (float)transformedRotationVec.Z;
        thisGyro.GyroOverride = true;
    }
}