File content as of revision 2:dc387d9de7c4:

#include "mbed.h"

// inputs EMG
AnalogIn emg0( A0 );
AnalogIn emg1( A1 );
AnalogIn emg3( A2 );

// Variabelen EMG
const double m1 =0.5000;
const double m2 =-0.8090;
const double n0 =0.5000;
const double n1 =-0.8090;
const double n2 =0;
const double a1 =0.9565;
const double a2 =-1.9131;
const double b0 =0.9565;
const double b1 =-1.9112;
const double b2 =0.9150;
const double c1 =0.0675;
const double c2 =0.1349;
const double d0 =0.0675;
const double d1 =-1.1430;
const double d2 =0.4128;

double highpassFilter1 = 0;
double lowpassFilter1 = 0;
double highpassFilter2 = 0;
double lowpassFilter2 = 0;

// Filteren
void filterEMG()
{   
notchFilter1 = biquad(emg0.read(), m1, m2, n0, n1, n2);
highpassFilter1 = fabs(biquad(notchFilter1, a1, a2, b0, b1, b2));
lowpassFilter1 = biquad(highpassFilter1, c1, c2, d0, d1, d2);
notchFilter2 = biquad(emg1.read(), m1, m2, n0, n1, n2);
highpassFilter2 = fabs(biquad(notchFilter2, a1, a2, b0, b1, b2));
lowpassFilter2 = biquad(highpassFilter2, c1, c2, d0, d1, d2);
notchFilter3 = biquad(emg2.read(), m1, m2, n0, n1, n2);
highpassFilter3 = fabs(biquad(notchFilter2, a1, a2, b0, b1, b2));
lowpassFilter3 = biquad(highpassFilter3, c1, c2, d0, d1, d2);
}

//calibrate (hier moet iets met maximale waarden enzo
dempelwaarde = int 0.5 * max_lowpassFilter3 

//Direction control
enum direction {Pos_RB, Pos_LB, Pos_RO, Pos_LO};
direction currentdirection = Pos_RB;
bool directionchanged = true;

int main()
{
    while ()
    {
        switch (currentdirection)
        {
            case Pos_RB:
            
            lowpassFilter1 = lowpassFilter1;
            lowpassFilter2 = lowpassFilter2;
            
            if (lowpassFilter3 > drempelwaarde)
            {
                currentdirection == Pos_LB;
                cirectionchanged = true;
                }
            break;    
            
            case Pos_LB:
            
            lowpassFilter1 = lowpassFilter1 * int -1;
            lowpassFilter2 = lowpassFilter2;
            
            if (lowpassFilter3 > drempelwaarde)
            
            {
                currentdirection == Pos_RO;
                cirectionchanged = true;
                }
            break;
                        
            case Pos_RO:
            
            lowpassFilter1 = lowpassFilter1;
            lowpassFilter2 = lowpassFilter2 * int -1;
                    
            if (lowpassFilter3 > drempelwaarde)
            {
                currentdirection == Pos_LO;
                cirectionchanged = true;
                }
            break;
                
            case Pos_LO:
            
            lowpassFilter1 = lowpassFilter1 * int -1;
            lowpassFilter2 = lowpassFilter2 * int -1;
            
            if (lowpassFilter3 > drempelwaarde)
            {
                currentdirection == Pos_RB;
                cirectionchanged = true;
                }
            break;
}