Example Gyroscopic Control Code

//Whip's Example Code for Gyroscopic Control - 12/28/17

double proportionalGain = 1; //modifier that changes how fast the ship will rotate

List<IMyGyro> gyros = new List<IMyGyro>();

void Main()
{
    GridTerminalSystem.GetBlocksOfType(gyros);

    IMyTerminalBlock referenceBlock = GridTerminalSystem.GetBlockWithName("Reference Block");
    if (referenceBlock == null)
        return;

    //this is a world direction vector that you wish to align the "forward" direction of the reference block to
    Vector3D desiredVector = new Vector3D(1, 2, 3);

    //get pitch and yaw angles (radians) required to steer onto desiredVector
    double pitch = 0; double yaw = 0;
    GetRotationAngles(desiredVector, referenceBlock, out yaw, out pitch);

    //the angular speed is obtained with a simple proportional error controller
    double pitchSpeed = proportionalGain * pitch;
    double yawSpeed = proportionalGain * yaw;

    //apply pitch and yaw speed to gyros
    ApplyGyroOverride(pitchSpeed, yawSpeed, 0, gyros, referenceBlock);
}

/*
/// Whip's Get Rotation Angles Method v10 - 12/27/17 ///
Dependencies: VectorProjection() | VectorAngleBetween()
* Fix to solve for zero cases when a vertical target vector is input
* Fixed straight up case
*/
void GetRotationAngles(Vector3D v_target, IMyTerminalBlock reference, out double yaw, out double pitch)
{
    var v_front = reference.WorldMatrix.Forward;
    var v_left = reference.WorldMatrix.Left;
    var v_up = reference.WorldMatrix.Up;

    var projectTargetUp = VectorProjection(v_target, v_up);
    var projTargetFrontLeft = v_target - projectTargetUp;

    yaw = VectorAngleBetween(v_front, projTargetFrontLeft);

    if (Vector3D.IsZero(projTargetFrontLeft) && !Vector3D.IsZero(projectTargetUp)) //check for straight up case
        pitch = MathHelper.PiOver2;
    else
        pitch = VectorAngleBetween(v_target, projTargetFrontLeft); //pitch should not exceed 90 degrees by nature of this definition

    //---Check if yaw angle is left or right
    //multiplied by -1 to convert from right hand rule to left hand rule
    yaw = -1 * Math.Sign(v_left.Dot(v_target)) * yaw;

    //---Check if pitch angle is up or down
    pitch = Math.Sign(v_up.Dot(v_target)) * pitch;

    //---Check if target vector is pointing opposite the front vector
    if (Math.Abs(yaw) <= 1E-6 && v_target.Dot(v_front) < 0)
    {
        yaw = Math.PI;
    }
}

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); //because keen does some weird stuff with signs
    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;
        thisGyro.Yaw = (float)transformedRotationVec.Y;
        thisGyro.Roll = (float)transformedRotationVec.Z;
        thisGyro.GyroOverride = true;
    }
}