Poep Hoofd / Mbed 2 deprecated PoolRobot_Code

Dependencies:   HIDScope mbed MODSERIAL QEI

main.cpp

Committer:
poephoofd
Date:
2017-10-16
Revision:
3:9c5aea210f1d
Parent:
2:a08bff88216d
Child:
5:4c27dea81e4c

File content as of revision 3:9c5aea210f1d:

#include "mbed.h"
#include"BiQuad.h"
#include "HIDScope.h"
#include "MODSERIAL.h"

//Initialize Analog EMG inputs
AnalogIn    EMGData_1( A0 );
AnalogIn    EMGData_2( A1 );
AnalogIn    EMGData_3( A2 );
AnalogIn    EMGData_4( A3 );

HIDScope scope(4); // 4 channels of data
Ticker MainTicker;
MODSERIAL pc(USBTX, USBRX);

const double Ts= 0.002;   //fs = 500Hz

volatile double EMG_MainsReject_1 = 0;
volatile double EMG_MainsReject_2 = 0;
volatile double EMG_MainsReject_3 = 0;
volatile double EMG_MainsReject_4 = 0;

volatile double EMG_Filtered_1 = 0;
volatile double EMG_Filtered_2 = 0;
volatile double EMG_Filtered_3 = 0;
volatile double EMG_Filtered_4 = 0;

volatile double EMG_Abs_1 = 0;
volatile double EMG_Abs_2 = 0;
volatile double EMG_Abs_3 = 0;
volatile double EMG_Abs_4 = 0;

volatile double EMG_Envelope_1 = 0;
volatile double EMG_Envelope_2 = 0;
volatile double EMG_Envelope_3 = 0;
volatile double EMG_Envelope_4 = 0;

//High Pass Filter
double a_0_0=1,                        a_0_1=-1.475480443592646,          a_0_2=0.586919508061190,
       b_0_0=0.765599987913459,        b_0_1=-1.531199975826918,          b_0_2=0.765599987913459;
           
BiQuad HiPass(b_0_0, b_0_1, b_0_2, a_0_0, a_0_1, a_0_2);
/***********/
    
//Low Pass Filter
double a_1_0=1,                        a_1_1=-1.307285028849324,          a_1_2=0.491812237222575,
       b_1_0=0.0009446918438401619,    b_1_1=0.001889383687680,           b_1_2=0.0009446918438401619;
           
BiQuad LoPass(b_1_0, b_1_1, b_1_2, a_1_0, a_1_1, a_1_2);
/***********/
    
//Notch Filter
double a_2_0=1,                        a_2_1=-1.525271192436899,          a_2_2=0.881618592363190,
       b_2_0=0.940809296181595,        b_2_1=-1.525271192436899,          b_2_2=0.940809296181595;
           
BiQuad MainsReject(b_2_0, b_2_1, b_2_2, a_2_0, a_2_1, a_2_2);
/***********/

void ReadAndFilterEMG()
{    
    //MainsReject Filter EMG Data
    EMG_MainsReject_1 = MainsReject.step(EMGData_1);
    EMG_MainsReject_2 = MainsReject.step(EMGData_2);
    EMG_MainsReject_3 = MainsReject.step(EMGData_3);
    EMG_MainsReject_4 = MainsReject.step(EMGData_4);
    /*****/
    
    //High Pass Filter EMG Data
    EMG_Filtered_1 = HiPass.step(EMG_MainsReject_1);
    EMG_Filtered_2 = HiPass.step(EMG_MainsReject_2);
    EMG_Filtered_3 = HiPass.step(EMG_MainsReject_3);
    EMG_Filtered_4 = HiPass.step(EMG_MainsReject_4);
    /*****/
    
    //Abs Filtered EMG Data
    EMG_Abs_1 = abs(EMG_Filtered_1);
    EMG_Abs_2 = abs(EMG_Filtered_2);
    EMG_Abs_3 = abs(EMG_Filtered_3);
    EMG_Abs_4 = abs(EMG_Filtered_4);
    /*****/
    
    //Low Pass Filter
    EMG_Envelope_1 = LoPass.step(EMG_Abs_1);
    EMG_Envelope_2 = LoPass.step(EMG_Abs_2);
    EMG_Envelope_3 = LoPass.step(EMG_Abs_3);
    EMG_Envelope_4 = LoPass.step(EMG_Abs_4);
    /*****/
    
    //Send scope data
    scope.set(0, EMGData_1);    //Raw Data
    scope.set(1, EMG_Filtered_1);   //Notch and High Pass Filtered
    scope.set(2, EMG_Abs_1);    //Absolute value
    scope.set(3, EMG_Envelope_1);   //Envelope Detected output
    
    scope.send();
    /*****/ 
}

int main(void)
{
    pc.baud(115200);    //Set Baud rate for Serial communication
    MainTicker.attach(&ReadAndFilterEMG, Ts);    //Attach time based interrupt
    
    /*
    while(true)
    {
        
    }
    */
    
    return 0;
}