File content as of revision 5:a53081a119c0:

#include "mbed.h"
#include "HIDScope.h"
#include "QEI.h"
#include "MODSERIAL.h"
#include "BiQuad.h"
#include "FastPWM.h"
#include "math.h"
#include <stdio.h>      /* printf */
#include <stdlib.h>     /* abs */
#include <complex>
#include "FastPWM.h"

MODSERIAL pc(USBTX, USBRX);

//Filters EMG
//Filters Linker Biceps
//In de volgorde High pass, notch, low pass
//BiQuad LBHP1(1, 2.000000023182220, 0.999999981993818, 1.893415601022358, 0.896255659548714);
//BiQuad LBHP2(1, 1.999999976817780, 0.999999976817780, 1.778313488139574, 0.790599715524785);
BiQuad LBHP1(0.995566972017647,  -1.991133944035294,   0.995566972017647, 1.000000000000000,  -1.991114292201654,   0.991153595868935);
//BiQuad LBHP2([3.913020539916823e-05,7.826041079833645e-05,3.913020539916823e-05, -1.982228929792529,0.982385450614126]);
BiQuad LBN1( 0.5, 0, 0.5, 0, 0);
BiQuad LBLP1(0000.087655548754006,   0000.175311097508013,   0000.087655548754006,    1.000000000000000,  -1.973344249781299,   0.973694871976315);
//BiQuad LBLP2( 0.000155148423475721, 0.000310296846951441, 0.000155148423475721,-1.96446058020523, 0.965081173899135);
BiQuadChain LeftBicepHP;
BiQuadChain LeftBicepN;
BiQuadChain LeftBicepLP;

//Filters Rechter Biceps
//In de volgorde High pass, notch, low pass
//BiQuad RBHP1(1, 2.000000023182220, 0.999999981993818, 1.893415601022358, 0.896255659548714);
//BiQuad RBHP2(1, 1.999999976817780, 0.999999976817780, 1.778313488139574, 0.790599715524785);
BiQuad RBHP1(0.995566972017647,  -1.991133944035294,   0.995566972017647, 1.000000000000000,  -1.991114292201654,   0.991153595868935);
//BiQuad RBHP2([3.913020539916823e-05,7.826041079833645e-05,3.913020539916823e-05, -1.982228929792529,0.982385450614126]);
BiQuad RBN1( 0.5, 0, 0.5, 0, 0);
BiQuad RBLP1(0000.087655548754006,   0000.175311097508013,   0000.087655548754006,  1.000000000000000,  -1.973344249781299,   0.973694871976315);
//BiQuad RBLP2( 0.000155148423475721, 0.000310296846951441, 0.000155148423475721,-1.96446058020523, 0.965081173899135);
BiQuadChain RightBicepHP;
BiQuadChain RightBicepN;
BiQuadChain RightBicepLP;


//Filters Rechter Quadriceps
//In de volgorde High pass, notch, low pass
//BiQuad RTHP1(1, 2.000000023182220, 0.999999981993818, 1.893415601022358, 0.896255659548714);
//BiQuad RTHP2(1, 1.999999976817780, 0.999999976817780, 1.778313488139574, 0.790599715524785);
BiQuad RTHP1(0.995566972017647,  -1.991133944035294 ,  0.995566972017647 ,1.000000000000000,  -1.991114292201654 ,  0.991153595868935);
//BiQuad RTHP2(1, 1.999999966458334, 0.999999966458334, -1.988418014198592, 0.988451549797368);
BiQuad RTN1( 0.5, 0, 0.5, 0, 0);
BiQuad RTLP1(0000.087655548754006  , 0.175311097508013 ,  0.087655548754006,  1.000000000000000 , -1.973344249781299  , 0.973694871976315);
//BiQuad RTLP2( 0.000155148423475721, 0.000310296846951441, 0.000155148423475721,-1.96446058020523, 0.965081173899135);
BiQuadChain RightLegHP;
BiQuadChain RightLegN;
BiQuadChain RightLegLP;

double emgLBHP;
double emgLBN;
double emgLBA;
//double emgLBLP;
//double emgAbsLBNotch;

double emgRBHP;
double emgRBN;
double emgRBA;
double emgRBLP;
double emgAbsRBNotch;

double emgRTN;
double emgRTHP;
double emgRTA;
//double emgRTLP;
//double emgAbsRTNotch;

double emgLBfiltered;
double emgRBfiltered;
double emgRTfiltered;
double emgLBraw;
double emgRBraw;
double emgRTraw;


double threshold_emgLB;
double threshold_emgRB;
double threshold_emgRT;
double threshold = 0.25;
double emgLB_max = 0.000;
double emgRB_max = 0.000;
double emgRT_max = 0.000;
double emgLB_maxrust = 0.000;
double emgRB_maxrust = 0.000;
double emgRT_maxrust = 0.000;
int tijd = 0;
double timecalibration;

double emgsumLB;
double emgsumRB;
double emgsumRT;
double restmeanLB = 0.000;
double restmeanRB = 0.000;
double restmeanRT = 0.000;
//double emgmeansubLB;
//double emgmeansubRB;
//double emgmeansubRT;
double LBvalue;
double RBvalue;
double RTvalue;
double emgLBrust;
double emgRBrust;
double emgRTrust;
double RestmeanLB;
double RestmeanRB;
double RestmeanRT;



const bool clockwise = true;
volatile bool direction1 = clockwise;
volatile bool direction2 = clockwise;

double maxPWM1 = 0.2;
double maxPWM2 = 0.2;
    
DigitalOut led5(LED_RED);

DigitalOut motor1_direction(D4);                                                //richting van motor1
FastPWM motor1_pwm(D5);                                                         //Motor 1 PWM controle van de snelheid
DigitalOut motor2_direction(D7);                                                //richting van motor2
FastPWM motor2_pwm(D6);                                                         //Motor 2 PWM controle van de snelheid

AnalogIn emgLB(A0);     //read EMG left bicep
AnalogIn emgRB(A1);     //read EMG right bicep
AnalogIn emgRT(A2);     //read EMG right quadriceps

HIDScope scope(6);      //aantal kanalen HIDScope (voor test 5, voor echt 6)

Ticker filter;
Ticker calibreren;
Ticker actie;

void Filteren()
{
    //linkerbicep
    emgLBraw= emgLB.read(); //dit wordt: emgLBoffset
    emgLBHP=LeftBicepHP.step(emgLBraw);
    emgLBN=LeftBicepN.step(emgLBHP);
    //emgmeansubLB = emgLBN - RestmeanLB;
    emgLBA= fabs(emgLBN);
    emgLBfiltered=LeftBicepLP.step(emgLBA);
    //LBvalue = emgLBfiltered/emgLB_max;

    //to show if filter is working
    scope.set(0, emgLBraw);
    scope.set(1, emgLBfiltered);
    scope.set(2, emgRBraw);
    scope.set(3, emgRBfiltered);
    scope.set(4, emgRTraw);
    scope.set(5, emgRTfiltered); 


    //rechterbicep
    emgRBraw= emgRB.read();
    emgRBHP= RightBicepHP.step(emgRBraw);
    emgRBN=RightBicepN.step(emgRBHP);
    //emgmeansubRB = emgRBN - RestmeanRB;
    emgRBA= fabs(emgRBN);
    emgRBfiltered=RightBicepLP.step(emgRBA);
    //RBvalue = emgRBfiltered/emgRB_max;

    //Right Quadriceps
    emgRTraw= emgRT.read();
    emgRTHP= RightLegHP.step(emgRTraw);
    emgRTN= RightLegN.step(emgRTHP);
   // emgmeansubRT = emgRTHP - RestmeanRT;
    emgRTA= fabs(emgRTN);
    emgRTfiltered=RightLegLP.step(emgRTA);
    //RTvalue = emgRTfiltered/emgRT_max;
    scope.send();

}

void calibration()
{
    
        emgLBraw= emgLB.read(); //dit wordt: emgLBoffset
        emgLBHP=LeftBicepHP.step(emgLBraw);
        emgLBN=LeftBicepN.step(emgLBHP);
        emgLBA= fabs(emgLBN);
        emgLBfiltered=LeftBicepLP.step(emgLBA);

        emgRBraw= emgRB.read();
        emgRBHP= RightBicepHP.step(emgRBraw);
        emgRBN=RightBicepN.step(emgRBHP);
        emgRBA= fabs(emgRBN);
        emgRBfiltered=RightBicepLP.step(emgRBA);
    

        emgRTraw= emgRT.read();
        emgRTHP= RightLegHP.step(emgRTraw);
        emgRTN= RightLegN.step(emgRTHP);
        emgRTA= fabs(emgRTN);
        emgRTfiltered=RightLegLP.step(emgRTA);

    if (tijd > 1000 && tijd < 6000) {
        emgLBraw= emgLB.read();
        emgLBHP=LeftBicepHP.step(emgLBraw);
        emgLBN=LeftBicepN.step(emgLBHP);
        //emgmeansubLB = emgLBN - restmeanLB;
        emgLBA= fabs(emgLBN);
        emgLBfiltered=LeftBicepLP.step(emgLBA);
        if (emgLBfiltered > emgLB_max) {
            emgLB_max = emgLBfiltered;
        }
        //pc.baud(115200);
        //pc.printf("emgLB_max = %f \r\n", emgLB_max);// geen tekst printen in ticker want dat gaat mis xjes
        led5=0; //led gaat aan zodra je linkerbicep moet aanspannen
    }

    else if (tijd > 7000 && tijd < 12000) {
        emgRBraw= emgRB.read();
        emgRBHP= RightBicepHP.step(emgRBraw);
        emgRBN=RightBicepN.step(emgRBHP);
        //emgmeansubRB = emgRBN - RestmeanRB;
        emgRBA= fabs(emgRBN);
        emgRBfiltered=RightBicepLP.step(emgRBA);
        if (emgRBfiltered > emgRB_max) {
            emgRB_max = emgRBfiltered;
        }
        //pc.printf("emgRB_max = %f \r\n", emgRB_max);
        led5=0; //led gaat uit zodra je rechterbicep moet aanspannen
    }


    else if (tijd > 13000 && tijd < 18000) {
        emgRTraw= emgRT.read();
        emgRTHP= RightLegHP.step(emgRTraw);
        emgRTN= RightLegN.step(emgRTHP);
        //emgmeansubRT = emgRTHP - RestmeanRT;
        emgRTA= fabs(emgRTN);
        emgRTfiltered=RightLegLP.step(emgRTA);
        if (emgRTfiltered > emgRT_max) {
            emgRT_max = emgRTfiltered;
        }
        //pc.printf("emgRT_max = %f \r\n", emgRT_max);
        led5=0; //led gaat aan zodra je rechtertricep moet aanspannen
    } else {
        led5=1; //led gaat uit zodra kalibratie voltooid
    }
    tijd= tijd + 1;    //idealiter op een andere plek haha
    
threshold_emgLB = threshold*emgLB_max;   
threshold_emgRB = threshold*emgRB_max; 
threshold_emgRT = threshold*emgRT_max;

// if threshold_emgx is reached, brushingmodes activated
}

void brushingmode()
{
    
//boven draait arm 1 aan, dus motor 1 
//onder draait arm 2 aan, dus motor 2 
if (emgLBfiltered > threshold_emgLB){      //tandenborstel naar links (cw/ccw) //direction1/2 = cw/ccw
motor2_direction.write(direction1);   //motor 2 gaat cw
//motor2_direction.write(direction1 = !direction1); //is counterclockwise
motor2_pwm.write(maxPWM2);

}else if (emgRBfiltered > threshold_emgRB){
motor2_direction.write(direction1 = !direction1); //is counterclockwise
motor2_pwm.write(maxPWM2);
//tandenborstel naar rechts

}else if (emgRTfiltered > threshold_emgRT){
motor1_direction.write(direction1 = !direction1);
motor1_pwm.write(maxPWM1);
//tandenborstel naar achter

}else if (emgRBfiltered > threshold_emgRB && emgLBfiltered > threshold_emgLB){
motor1_direction.write(direction1); //motor 1 gaat cw
motor1_pwm.write(maxPWM1);
//tandenborstel naar voren

}
}
int main()
{
    // -------------------- Serial Comms --------------------------------
    pc.baud(115200);
    pc.printf("Hello");

    // -------------------- BiQuad Chains -------------------------------
    LeftBicepHP.add( &LBHP1 );  //BiQuadChain bqc;   //bqc.add( &bq1 ).add( &bq2 );
    LeftBicepN.add( &LBN1 );
    LeftBicepLP.add( &LBLP1 );


    RightBicepHP.add( &RBHP1 );
    RightBicepN.add( &RBN1 );
    RightBicepLP.add( &RBLP1 );

    RightLegHP.add( &RTHP1 );
    RightLegN.add( &RTN1 );
    RightLegLP.add( &RTLP1 );

    // -------------------- Tickers -------------------------------------

    calibreren.attach(calibration, 0.001f);
    filter.attach(Filteren, 0.001f); //ticker aanroepen van filter
    actie.attach(brushingmode, 0.001f); 
    
}