File content as of revision 18:898f54c6aa3d:

#include "mbed.h"
#include "MODSERIAL.h"
#include "BiQuad.h"
#include "HIDScope.h"
#include <math.h>

AnalogIn emg0_in            (A0);
AnalogIn emg1_in            (A1);
AnalogIn emg2_in            (A2);

InterruptIn button1         (D10);                  //Let op, is deze niet bezet? En 11? Even checken, als er een error komt, kan het hier zitten.
InterruptIn button2         (D11);
InterruptIn encoderA        (D9);
InterruptIn encoderB        (D8);

DigitalOut directionpin1    (D4);
DigitalOut directionpin2    (D7);
DigitalOut ledr             (LED_RED);
DigitalOut ledb             (LED_BLUE);
DigitalOut ledg             (LED_GREEN);

PwmOut pwmpin1              (D5);
PwmOut pwmpin2              (D6);


MODSERIAL pc(USBTX, USBRX);


//Global variables
int encoder     = 0;                   //Starting point encoder
const float T   = 0.001f;              //Ticker period

//EMG filter
double emgfilter0, emgfilter1, emgfilter2;                                                       //Filtered EMG data 0, 1 and 2
const int windowsize = 150;                                                                         //Size of the array over which the moving average (MovAg) is calculated
double sum, sum1, sum2, sum3;                                                                    //variables used to sum elements in array
double StoreArray0[windowsize], StoreArray1[windowsize], StoreArray2[windowsize];  //Empty arrays to calculate MoveAg
double movAg0,movAg1,movAg2;                                                                     //outcome of MovAg

//calibration
static int x = -1;
int emg_cal = 0;
const int sizeCal = 2000;
double StoreCal0[sizeCal], StoreCal1[sizeCal], StoreCal2[sizeCal];
double Mean0,Mean1,Mean2;
double Threshold0 = 1, Threshold1 = 1, Threshold2 = 1;


//Biquad
BiQuadChain emg0band;
BiQuad emg0band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
BiQuad emg0band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
BiQuad emg0band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );

BiQuadChain emg1band;
BiQuad emg1band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
BiQuad emg1band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
BiQuad emg1band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );

BiQuadChain emg2band;
BiQuad emg2band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
BiQuad emg2band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
BiQuad emg2band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );

BiQuad notch1( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 );                //Notch filter
BiQuad notch2( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 );                //Notch filter
BiQuad notch3( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 );                //Notch filter


//Tickers
Ticker filter_tick;
Ticker MovAg_tick;

//Functions
void EMGFilter0()
{
    double emg0          = emg0_in.read();
    double bandpass0     = emg0band.step(emg0);
    double absolute0     = fabs(bandpass0);
    double notch0        = notch1.step(absolute0);
}

void EMGFilter1()
{
    double emg1          = emg1_in.read();
    double bandpass1     = emg1band.step(emg1);
    double absolute1     = fabs(bandpass1);
    double notch1        = notch2.step(absolute1);
}

void EMGFilter2()
{
    double emg2          = emg2_in.read();
    double bandpass2     = emg2band.step(emg2);
    double absolute2     = fabs(bandpass2);
    double notch2        = notch3.step(absolute2);
}

void MovAg()                                        //Calculate moving average (MovAg)
{
    for (int i = windowsize-1; i>=0; i--)            //Make array of the last datapoints of the filtered signal
    {
        StoreArray0[i] = StoreArray0[i-1];
        StoreArray1[i] = StoreArray1[i-1];
        StoreArray2[i] = StoreArray2[i-1];
    }
    
    StoreArray0[0] = emgfilter0;                    //Stores the latest datapoint in the first element of the array
    StoreArray1[0] = emgfilter1;
    StoreArray2[0] = emgfilter2;
    
    sum1 = 0.0;
    sum2 = 0.0;
    sum3 = 0.0;
    
    for(int a = 0; a<= windowsize-1; a++)            //Sum the elements in the array
    {
        sum1 += StoreArray0[a];
        sum2 += StoreArray1[a];
        sum3 += StoreArray2[a];
    }
    
    movAg0 = sum1/windowsize;                        //calculates an average in the array
    movAg1 = sum2/windowsize;
    movAg2 = sum3/windowsize;
}

void emg_filtered()             //Call all filter functions
{
    EMGFilter0();
    EMGFilter1();
    EMGFilter2();
    MovAg();
}

void switch_to_calibrate()
{
    x++;
    
    if(x==0)                    //If x = 0, led is red
    {
        ledr = 0;
        ledb = 1;
        ledg = 1;
    }
    else if (x==1)              //If x = 1, led is blue
    {
        ledr = 1;
        ledb = 0;
        ledg = 1;
    }
    else if (x == 2)            //If x = 2, led is green
    {
        ledr = 1;
        ledb = 1;
        ledg = 0;
    }
    else                        //If x = 3, led is white
    {
        ledr = 0;
        ledb = 0;
        ledg = 0;
    }
   
    if(x>=4)                    //Reset back to x = 0
    {
        x = -1;
    }
}
    
        
void calibrate(void)
{
    switch(x)
    {
        case 0: 
        {
            sum = 0.0;
            for(int j = 0; j<=sizeCal-1; j++)
            {
                StoreCal0[j] = emgfilter0;
                sum+=StoreCal0[j];
                //wait(0.001f);
            }
            Mean0       = sum/sizeCal;
            Threshold0  = Mean0/2;
            break;
        }
        case 1:
        {
            sum = 0.0;
            for(int j = 0; j<=sizeCal-1; j++)
            {
                StoreCal1[j] = emgfilter1;
                sum+=StoreCal1[j];
                //wait(0.001f);
            }
            Mean1       = sum/sizeCal;
            Threshold1  = Mean1/2;
            break;
        }
        case 2:
        {
            sum = 0.0;
            for(int j = 0; j<=sizeCal-1; j++)
            {
                StoreCal1[j] = emgfilter2;
                sum+=StoreCal2[j];
                //wait(0.001f);
            }
            Mean2       = sum/sizeCal;
            Threshold2  = Mean2/2;
            break;
        }
        case 3:                                     //EMG is calibrated, robot can be set to Home position.
        {
            emg_cal = 1;
            //wait(0.001f);
            break;
        }
        default:                                    //Ensures nothing happens if x is not 0,1 or 2.
        {
            break;
        }
    }
}
            
void encoderA_rise()       
{
    if(encoderB==false)
    {
        encoder++;
    }
    else
    {
        encoder--;
    }
}

void encoderA_fall()      
{
    if(encoderB==true)
    {
        encoder++;
    }
    else
    {
        encoder--;
    }
}

void encoderB_rise()       
{
    if(encoderA==true)
    {
        encoder++;
    }
    else
    {
        encoder--;
    }
}

void encoderB_fall()      
{
    if(encoderA==false)
    {
        encoder++;
    }
    else
    {
        encoder--;
    }
}


// Main function start.

int main()
{ 
    //pc.baud(115200);
    //pc.printf("hello\n\r");
    
    ledr = 0;       //Begin led = red, first state of calibration
    ledb = 1;
    ledg = 1;
    
    filter_tick.attach(&emg_filtered,T);        //EMG signals filtered + moving average every T sec.
    button1.rise(switch_to_calibrate);          //Switch state of calibration (which muscle)
    wait(0.2f);
    button2.rise(calibrate);                    //calibrate threshold for 3 muscles
    wait(0.2f);
            
    pwmpin1.period_us(60);                      //60 microseconds PWM period, 16.7 kHz 

    encoderA.rise(&encoderA_rise);
    encoderA.fall(&encoderA_fall);
    encoderB.rise(&encoderB_rise);
    encoderB.fall(&encoderB_fall);
    
    if(emg_cal==1)
    { 
         while (true)
                {
                //Motor aansturen en encoder uitlezen
                  //float u1 = potmetervalue1;
                  //float u2 = potmetervalue2;
                  
                  //float m1 = ((u1*2.0f)-1.0f);
                  //float m2 = ((u2*2.0f)-1.0f);
                
                  //pwmpin1 = fabs(m1*0.6f)+0.4f;     //pwm duty cycle can only be positive, floating, 0.4f is "inefficiënt", dit tellen we erbij op, en keer 0.6 om te corrigeren voor de helling.        
                
                if(emgfilter0>Threshold0)
                {
                       pwmpin1 = 1;
                       directionpin1.write(1);
                }
                else
                {
                       pwmpin1 = 0;
                }
                
                if(emgfilter1>Threshold1)
                {
                       pwmpin2 = 1;
                       directionpin2.write(1);
                }
                else
                {
                       pwmpin2 = 0;
                }
                if(emgfilter2>Threshold2)
                {
                       pwmpin1 = 1;
                       pwmpin2 = 2;
                       directionpin1.write(1);
                       directionpin2.write(1);
                }
                else
                {
                       pwmpin1 = 0;
                       pwmpin2 = 0;
                }
                       
                       //Indien waar, motor draait rechtsom. Indien niet waar, motor draait linksom. 
                //wait(0.01f);                   //zodat de code niet oneindig doorgaat.
                //pwmpin2 = fabs(m2*0.6f)+0.4f;    
                //directionpin2.write(m2>0);   
                
                //float encoderDegrees = float(encoder)*(360.0/8400.0);
                        
                //pc.printf("Encoder count: %f \n\r",encoderDegrees);
        
    }
}
}