File content as of revision 58:897b67113094:

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

    extern const 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);
        
}

float Biquad(ControllerSettings& StrucName, MemoryIO& MemName, float X)
{
    //Difference equation which implements the digital biquad filter
    float Y = StrucName.A * X + StrucName.B * MemName.Xm + StrucName.C * MemName.Xmm + StrucName.D * MemName.Ym + StrucName.E * MemName.Ymm;        
    
    
    return Y; 
}