File content as of revision 49:a9ed4f4cdef7:

#include "structures.h"
#include <math.h>

    extern float Ts;
    float K = 2/Ts;

/*
To call this function, the coefficients variable in the main scope needs to have a 
"*" in front of it. The coefficients can be printed using:

       for ( int i = 0; i < 5; i++ ) {
        std::cout << *(coefficient_variable_name+i) << std::endl;
        }

*/

void BuildController(ControllerSettings& StrucName, float P[], float Q[])
{
    
    StrucName.A = (Q[0] * pow(K,2) + Q[1] * K + Q[2])/(P[0] * pow(K,2) + P[1] * K + P[2]);
    StrucName.B = (2 * Q[2] - 2 * Q[0] * pow(K,2))/(P[0] * pow(K,2) + P[1] * K + P[2]);
    StrucName.C = (Q[0] * pow(K,2) - Q[1] * K + Q[2])/(P[0] * pow(K,2) + P[1] * K + P[2]);
    StrucName.D = (2 * P[2] - 2 * P[0] * pow(K,2))/(P[0] * pow(K,2) + P[1] * K + P[2]);
    StrucName.E = (P[0] * pow(K,2) - P[1] * K + P[2])/(P[0] * pow(K,2) + P[1] * K + P[2]);;
}

void InitializeControllers(void)
{
    //Define Controller structures, shorthand: Set_
    ControllerSettings Set_ProcessEMG;
    ControllerSettings Set_Base;            //Controller base
    ControllerSettings Set_EndAffector;     //Controller end affector 
    
    //Define Memory cells of the Input/Output, shorthand: Mem_
    MemoryIO Mem_ProcessEMG;
    MemoryIO Mem_Base;
    MemoryIO Mem_EndAffector;
    
    
    //Build Controllers and define P and Q, the transfer function of the controller
    //--Build controller, EMG
        float PE[] = {1,2,3};
        float QE[] = {4,5,6};
        BuildController(Set_ProcessEMG, PE, QE);
    //--Build controller, Base
        float PB[] = {1,2,3};
        float QB[] = {4,5,6};
        BuildController(Set_Base, PB, QB);
    //--Build controller, End Affector
        float PA[] = {1,2,3};
        float QA[] = {4,5,6};
        BuildController(Set_EndAffector, PA, QA);
}