Rotor Slaving V001503062233

/*
*   Cats makes the best programmers!
*   Controls the velocity of a list of slave rotors to maintain equivalent rotation relative to a pair of master rotors
*   The difference in angle between RM1 and all RS1 and RS1i rotors is converted to a velocity and applied to the RS1 and RS1i rotors
*   The same is done for RM2 and RS2 and RS2i rotors.
*   The result should be that all the rotors maintain the same angle allowing them to be used in custom turrets
*   This script is very basic and unlikely to be directly useful. It is indented to be a starting point for further development.
*   http://steamcommunity.com/sharedfiles/filedetails/?id=392439615
*/

const string SHIP_PREFIX = null; // here for prefix filtering. if set to a string, the script will lokk for blocks whos name starts with that string, ignoring all others.

const string ROTOR_MASTER_ONE = "[RM1]"; // Base (hour angle) rotor master (Only one used for this script)
const string ROTOR_MASTER_TWO = "[RM2]"; // Second (Declination) rotor master (Only one used for this script)

const string ROTOR_SLAVE_ONE = "[RS1]"; // Base (hour angle) rotor slave (use as many as you like!)
const string ROTOR_SLAVE_TWO = "[RS2]"; // Second (Declination) rotor slave (use as many as you like!)
const string ROTOR_SLAVE_ONE_INV = "[RS1i]"; // Base (hour angle) rotor slave Inverted (use as many as you like!)
const string ROTOR_SLAVE_TWO_INV = "[RS2i]"; // Second (Declination) rotor slave Inverted (use as many as you like!)

const float SLOW = 5; // how much to slow down the turret rotation speed by (needed value depends on small or large ship)

bool set = false; // determines whether or not setup has been run.
IMyMotorStator[] Masters = null; // stores the 2 Master rotor items
List<IMyMotorStator> SlavesOne = null; // stores all slave one rotors
List<IMyMotorStator> SlavesOneInv = null; // stores all slave one inverse rotors
List<IMyMotorStator> SlavesTwo = null; // stores all slave two rotors
List<IMyMotorStator> SlavesTwoInv = null; // stores all slave two inverse rotors


// used once to find all the target rotors
public void Setup() {
    Masters = new IMyMotorStator[2];
    SlavesOne = new List<IMyMotorStator>();
    SlavesOneInv = new List<IMyMotorStator>();
    SlavesTwo = new List<IMyMotorStator>();
    SlavesTwoInv = new List<IMyMotorStator>();

    var blocks = new List<IMyTerminalBlock>();
    GridTerminalSystem.GetBlocksOfType<IMyMotorStator>( blocks );
    for( int e = 0; e < blocks.Count; e++ ) {
        blocks[e].SetValueFloat( "Velocity", 0 );
        if( SHIP_PREFIX == null || blocks[e].CustomName.StartsWith( SHIP_PREFIX ) ) {
            if( blocks[e].CustomName.Contains( ROTOR_SLAVE_ONE ) ) {
                SlavesOne.Add( (IMyMotorStator)blocks[e] );
            } else if ( blocks[e].CustomName.Contains( ROTOR_SLAVE_TWO ) ) {
                SlavesTwo.Add( (IMyMotorStator)blocks[e] );
            } else if( blocks[e].CustomName.Contains( ROTOR_SLAVE_ONE_INV ) ) {
                SlavesOneInv.Add( (IMyMotorStator)blocks[e] );
            } else if ( blocks[e].CustomName.Contains( ROTOR_SLAVE_TWO_INV ) ) {
                SlavesTwoInv.Add( (IMyMotorStator)blocks[e] );
            } else if ( blocks[e].CustomName.Contains( ROTOR_MASTER_ONE ) ) {
                Masters[0] = (IMyMotorStator)blocks[e];
            } else if ( blocks[e].CustomName.Contains( ROTOR_MASTER_TWO ) ) {
                Masters[1] = (IMyMotorStator)blocks[e];
            }
        }
    }

    if( Masters[0] == null || Masters[1] == null ) {
        throw new System.ArgumentException("Setup error", "Masters Not Found");
    }
}

// Update all slave rotor velocities to move them toward the Master rotor angle
public void Update() {
    float tarDeg = getAngle( Masters[0] );
    float curDeg = 0;
    for( int e = 0; e < SlavesOne.Count; e++ ) {
        curDeg = getAngle( SlavesOne[e] );
        SlavesOne[e].SetValueFloat( "Velocity", getNeededVel( tarDeg, curDeg ) / SLOW );
    }

    for( int e = 0; e < SlavesOneInv.Count; e++ ) {
        curDeg = getAngle( SlavesOneInv[e] );
        SlavesOneInv[e].SetValueFloat( "Velocity", getNeededVel( ((float)360)-tarDeg, curDeg ) / SLOW );
    }

    tarDeg = getAngle( Masters[1] );
    for( int e = 0; e < SlavesTwo.Count; e++ ) {
        curDeg = getAngle( SlavesTwo[e] );
        SlavesTwo[e].SetValueFloat( "Velocity", getNeededVel( tarDeg, curDeg ) / SLOW );
    }

    for( int e = 0; e < SlavesTwoInv.Count; e++ ) {
        curDeg = getAngle( SlavesTwoInv[e] );
        SlavesTwoInv[e].SetValueFloat( "Velocity", getNeededVel( ((float)360)-tarDeg, curDeg ) / SLOW );
    }
}

// just because it got repeated so often
public float getAngle( IMyMotorStator motor ) {
    string data = motor.DetailedInfo;
    string[] dataSplit = data.Split( ':' );
    return float.Parse( dataSplit[1].Substring( 0, dataSplit[1].Length-1 ) );
}

//  There is probably a better way to do this, but this appears to work.
//  I feel like there should be an elegant mathematical solution for this rather than if else
public float getNeededVel( float tDeg, float cDeg ) {
    float nVel = tDeg - cDeg;
    if( nVel > 180 ) {
        nVel = nVel - ((float)360);
    } else if( nVel < -180 ) {
        nVel = ((float)360) + nVel;
    }
    return nVel;
}


void Main() {
    if( set == false ) {
        Setup();
        set = true;
    } else {
        Update();
    }
}