File content as of revision 55:f764f223c56a:

#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;
        }

*/
/*
float * Filt(float P[], float Q[])
{

    static float coefficients[5];
        coefficients[0] = (Q[0] * pow(K,2) + Q[1] * K + Q[2])/(P[0] * pow(K,2) + P[1] * K + P[2]);
        coefficients[1] = (2 * Q[2] - 2 * Q[0] * pow(K,2))/(P[0] * pow(K,2) + P[1] * K + P[2]);
        coefficients[2] = (Q[0] * pow(K,2) - Q[1] * K + Q[2])/(P[0] * pow(K,2) + P[1] * K + P[2]);
        coefficients[3] = (2 * P[2] - 2 * P[0] * pow(K,2))/(P[0] * pow(K,2) + P[1] * K + P[2]);
        coefficients[4] = (P[0] * pow(K,2) - P[1] * K + P[2])/(P[0] * pow(K,2) + P[1] * K + P[2]);
    return coefficients;
} 
*/
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 DigitalBiquadFilter(ControllerSettings& StrucName, MemoryIO& MemName, 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; 
}