File content as of revision 67:5ebb08e337ae:

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

    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)
{
    //Build Controllers and define P and Q, the transfer function of the controller
    //--Build LPF controller, EMG
        float PL[] = {1,2,3};
        float QL[] = {4,5,6};
        BuildController(Set_LPFEMG, PL, QL);
    //--Build Notch filter controller, EMG
        float PN[] = {1,2,3};
        float QN[] = {4,5,6};
        BuildController(Set_NOTEMG, PN, QN);
    //--Build controller, Base
        float PB[] = {1,2,3};
        float QB[] = {4,5,6};
        BuildController(Set_Base, PB, QB);
    //--Build controller, End Affector
        float PE[] = {1,2,3};
        float QE[] = {4,5,6};
        BuildController(Set_EndAffector, PE, QE);
}

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;        
    
    MemName.ShiftValues(X,Y);
      
    return Y; 
}