File content as of revision 16:f7881b420c83:

#include "mbed.h"
#include "MODSERIAL.h"
#include "HIDScope.h"
#include <iostream>

PwmOut m_speed1(PTA5);
    DigitalOut m_dir1(PTA4);
    PwmOut m_speed2(PTC8);
    DigitalOut m_dir2(PTC9);    
    
    float s_speed1;
    float s_dir1;
    float s_speed2;
    float s_dir2;

#define A1LP    0.018180963222803
#define A0LP     0.016544013176248
#define B1LP    -1.718913340044714
#define B0LP     0.753638316443765

#define A1HP    -1.999801878951505
#define A0HP   0.999801878951505
#define B1HP   -1.971717601075000
#define B0HP  0.972111984032897

#define A0N 0.99436777112
#define A1N -1.89139989664
#define A2N 0.99436777112
#define B1N 1.89070035439
#define B2N -0.988036

#define TSAMP 0.001
#define TRESHOLD 0.03
AnalogIn    emg1(PTB1);
AnalogIn    emg2(PTB2);

MODSERIAL pc(USBTX,USBRX);
HIDScope scope(5);
Ticker timer;
volatile bool looptimerflag;

float emg_value2, ylp2, yhp2, yn2;
float emg_value1, ylp1, yhp1, yn1;

float ysum1 = 0, yave1=0 ;
float ysum2 = 0, yave2=0 ;

int set;
int y1, y2;

float readEMG1()
{

    emg_value1=emg1.read();
    return emg_value1;
}


float readEMG2()
{

    emg_value2=emg2.read();
    return emg_value2;
}

float notchfilter2(float ylp2)
{
    static float x1=0,x2=0,y1=0,y2=0,x;
    x = ylp2;
    yn2 = A0N*x + A1N*x1+A2N*x2+B1N*y1+B2N*y2;
    x2 = x1;
    x1 = x;
    y2 = y1;
    y1 = yn2;
    return yn2;
}

float notchfilter1(float ylp1)
{
    static float yn1,x1=0,x2=0,y1=0,y2=0,x;
    x = ylp1;
    yn1 = A0N*x + A1N*x1+A2N*x2+B1N*y1+B2N*y2;
    x2 = x1;
    x1 = x;
    y2 = y1;
    y1 = yn1;
    return yn1;
}

float hpfilter2(float yn2)
{
    static float x1=0,y1=0,x2=0, y2=0,x;
    x = yn2;
    yhp2 = x + A1HP*x1 + A0HP*x2 - B1HP*y1 - B0HP*y2;
    x2 = x1;
    x1 = x;
    y2 = y1;
    y1 = yhp2;
    return yhp2;
}
float hpfilter1(float yn1)
{
    static float x1=0,y1=0,x2=0, y2=0,x;
    x = yn1;
    yhp1 = x + A1HP*x1 + A0HP*x2 - B1HP*y1 - B0HP*y2;
    x2 = x1;
    x1 = x;
    y2 = y1;
    y1 = yhp1;
    return yhp1;
}

float lpfilter2(float yhp2)
{
    static float x1=0,y1=0,x2=0, y2=0,x;
    x = yhp2;
    ylp2 = A1LP*x1-B1LP*y1+A0LP*x2-B0LP*y2;
    x2 = x1;
    x1 = x;
    y2 = y1;
    y1 = ylp2;
    return ylp2;
}

float lpfilter1(float yhp1)
{
    static float x1=0,y1=0,x2=0, y2=0,x;
    x = yhp1;
    ylp1 = A1LP*x1-B1LP*y1+A0LP*x2-B0LP*y2;
    x2 = x1;
    x1 = x;
    y2 = y1;
    y1 = ylp1;
    return ylp1;
}

void viewer()
{
    scope.set(0,ylp1);
    scope.set(1,yave1);
    scope.set(2,ylp2);
    scope.set(3,yave2);  
    scope.set(4,y1);
    scope.send();
}

void setlooptimerflag(void)
{
    looptimerflag = true;
}

float filter1(float emg_value1)
{
        static int n;
        yn1 = notchfilter1(emg_value1);
        yhp1 = hpfilter1(yn1);  
        ylp1 = lpfilter1(yhp1);  
        ylp1 = fabs(ylp1);  
        ysum1 = ysum1+ylp1;
        n++;
                 
        if(n==200) {
            yave1 = ysum1/200;
            ysum1 = 0;
            n = 0;
        }
        return yave1;
}

float filter2(float emg_value2)
{
        static int n;
        yn2 = notchfilter2(emg_value2);
        yhp2 = hpfilter2(yn2);  
        ylp2 = lpfilter2(yhp2); 
        ylp2 = fabs(ylp2);
        ysum2 = ysum2 + ylp2;
        n++;
       
        if(n==100) {
            yave2 = ysum2/100;
            ysum2 = 0;
            n = 0;
        }
        return yave2;
}
        
int main()
{
    pc.baud(115200);
    timer.attach(setlooptimerflag,TSAMP);
     m_speed1=0;
        m_speed2=0;
    
    
    while(1) {
        while(!looptimerflag);
        
        looptimerflag = false;
        
        emg_value1 =  readEMG1();  
        emg_value2 =  readEMG2();   
        yave1 = filter1(emg_value1);
        yave2 = filter2(emg_value2);
        
        if(yave1>TRESHOLD){
                  y1 = 1;
                    cout<<y1<<endl;  
                    m_speed1=y1;
                    m_dir1=0;           
                  }
        else{
                  y1 = 0;
                  cout<<y1<<endl;
                  m_speed1=y1;
                  m_dir1=1;
                  }
        if(yave2>TRESHOLD){
                  y2 = 1;
                  
                  }
        else{
                  y2 = 0;
                  }
         
               
                  
                  
       viewer();               
    }
}