Poep Hoofd / Mbed 2 deprecated PoolRobot_Code

Dependencies:   HIDScope mbed MODSERIAL QEI

main.cpp

Committer:
poephoofd
Date:
2017-10-13
Revision:
1:50ae89200a53
Parent:
0:2d9dae739559
Child:
2:a08bff88216d

File content as of revision 1:50ae89200a53:

#include "mbed.h"
#include"BiQuadFilter.h"
#include "HIDScope.h"

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

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

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

double EMG_MainsReject_1 = 0;
double EMG_MainsReject_2 = 0;
double EMG_MainsReject3 = 0;
double EMG_Filtered_1 = 0;
double EMG_Filtered_2 = 0;
double EMG_Filtered_3 = 0;
double EMG_Abs_1 = 0;
double EMG_Abs_2 = 0;
double EMG_Abs_3 = 0;
double EMG_Envelope_1 = 0;
double EMG_Envelope_2 = 0;
double EMG_Envelope_3 = 0;

int main(void)
{
    pc.baud(115200);    //Set Baud rate for Serial communication
    MainTicker.attach(&ReadAndFilterEMG, Ts);    //Attach time based interrupt
    
    //High Pass Filter
    double A0=1,                        A1=-1.475480443592646,          A2=0.586919508061190,
           B0=0.765599987913459,        B1=-1.531199975826918,          B2=0.765599987913459;
           
    BiQuad HiPass(B0, B1, B2, A0, A1, A2);
    /***********/
    
    //Low Pass Filter
    double A0=1,                        A1=-1.307285028849324,          A2=0.491812237222575,
           B0=0.0009446918438401619,    B1=0.001889383687680,           B2=0.0009446918438401619;
           
    BiQuad LoPass(B0, B1, B2, A0, A1, A2);
    /***********/
    
    //Notch Filter
    double A0=1,                        A1=-1.525271192436899,          A2=0.881618592363190,
           B0=0.940809296181595,        B1=-1.525271192436899,          B2=0.940809296181595;
           
    BiQuad MainsReject(B0, B1, B2, A0, A1, A2);
    /***********/
    
    while(true)
    {
        
    }
    
    return 0;
}

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);
    /*****/
    
    //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);
    /*****/
    
    //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);
    /*****/
    
    //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);
    /*****/
    
    //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();
    /*****/
    
}