File content as of revision 24:094e719f38f3:

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

MODSERIAL pc(USBTX,USBRX);

HIDScope scope(4);

AnalogIn emgB(PTB1); //biceps
AnalogIn emgT(PTB2); //tricep


//*** OBJECTS ***
//bicep
uint16_t emg_valueB;
float emg_value_f32B;
float filtered_emgB;
float drempelwaardeB1, drempelwaardeB2, drempelwaardeB3;//B1=snelheidsstand 1, B2=snelheidsstand 2, B3=snelheidsstand 3
int yB1, yB2, yB3;
float B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, B10, B11, B12, B13, B14, B15, B16, B17, B18, B19, B20, B21, B22, B23, B24, B25, B26, B27, B28, B29, MOVAVG_B; //moving average objects
float B30, B31, B32, B33, B34, B35, B36, B37, B38, B39, B40, B41, B42, B43, B44, B45, B46, B47, B48, B49, B50, B51, B52, B53, B54, B55, B56, B57, B58, B59;
int snelheidsstand;
//tricep
uint16_t emg_valueT;
float emg_value_f32T;
float filtered_emgT;
float drempelwaardeT;
int yT1, yT2;
float T0, T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29,MOVAVG_T; //moving average objects
float MOVAVG_Positie1, MOVAVG_Positie2;
int positie;


//*** FILTERS ***
arm_biquad_casd_df1_inst_f32 notchT;
arm_biquad_casd_df1_inst_f32 notchB;
//constants for 50Hz
float notch_const[]= {0.5857841106784856, -1.3007020142696517e-16, 0.5857841106784856, 1.3007020142696517e-16, -0.17156822135697122}; //{a0 a1 a2 -b1 -b2}
float notch_states[4];

arm_biquad_casd_df1_inst_f32 lowpassT;
arm_biquad_casd_df1_inst_f32 lowpassB;
//constants for 60Hz lowpass
float lowpass_const[] = {0.39133426347022965, 0.7826685269404593, 0.39133426347022965, -0.3695259524151476, -0.19581110146577096};//{a0 a1 a2 -b1 -b2} van online calculator
float lowpass_states[4];

arm_biquad_casd_df1_inst_f32 highpassT;
arm_biquad_casd_df1_inst_f32 highpassB;
//constants for 20Hz highpass
float highpass_const[] = {0.6389437261127494, -1.2778874522254988, 0.6389437261127494, 1.1429772843080923, -0.41279762014290533};//{a0 a1 a2 -b1 -b2}
float highpass_states[4];


// *** TRICEPS en BICEPS EMG ***
void Triceps()
{
    //Triceps lezen
    emg_valueT = emgT.read_u16();
    emg_value_f32T = emgT.read();

    //Triceps filteren
    arm_biquad_cascade_df1_f32(&notchT, &emg_value_f32T, &filtered_emgT, 1);
    arm_biquad_cascade_df1_f32(&lowpassT, &filtered_emgT, &filtered_emgT, 1 );
    filtered_emgT = fabs(filtered_emgT);
    arm_biquad_cascade_df1_f32(&highpassT, &filtered_emgT, &filtered_emgT, 1 );
    filtered_emgT = fabs(filtered_emgT);

    //Triceps moving average
    T0=filtered_emgT*100;
    MOVAVG_T=T0*0.03333+T1*0.03333+T2*0.03333+T3*0.03333+T4*0.03333+T5*0.03333+T6*0.03333+T7*0.03333+T8*0.03333+T9*0.03333+T10*0.03333+T11*0.03333+T12*0.03333+T13*0.03333+T14*0.03333+T15*0.03333+T16*0.03333+T17*0.03333+T18*0.03333+T19*0.03333+T20*0.03333+T21*0.03333+T22*0.03333+T23*0.03333+T24*0.03333+T25*0.03333+T26*0.03333+T27*0.03333+T28*0.03333+T29*0.03333;
    T29=T28;
    T28=T27;
    T27=T26;
    T26=T25;
    T25=T24;
    T24=T23;
    T23=T22;
    T22=T21;
    T21=T20;
    T20=T19;
    T19=T18;
    T18=T17;
    T17=T16;
    T16=T15;
    T15=T14;
    T14=T13;
    T13=T12;
    T12=T11;
    T11=T10;
    T10=T9;
    T9=T8;
    T8=T7;
    T7=T6;
    T6=T5;
    T5=T4;
    T4=T3;
    T3=T2;
    T2=T1;
    T1=T0;

    //sturen naar scherm (Realterm)
    pc.printf("%f\r\n",MOVAVG_T);

    //sturen naar HID Scope
    scope.set(0,emg_valueT);        //ruwe data
    scope.set(1,filtered_emgT);     //filtered
    scope.send();
}

void Biceps()
{
    //Biceps lezen
    emg_valueB = emgB.read_u16(); // read direct ADC result, converted to 16 bit integer (0..2^16 = 0..65536 = 0..3.3V)
    emg_value_f32B = emgB.read();

    //Biceps filteren
    arm_biquad_cascade_df1_f32(&notchB, &emg_value_f32B, &filtered_emgB, 1 );
    arm_biquad_cascade_df1_f32(&lowpassB, &filtered_emgB, &filtered_emgB, 1 );
    filtered_emgB = fabs(filtered_emgB);
    arm_biquad_cascade_df1_f32(&highpassB, &filtered_emgB, &filtered_emgB, 1 );
    filtered_emgB = fabs(filtered_emgB);

    //Biceps moving average
    B0=filtered_emgB*1000;
    MOVAVG_B=B0*0.01667+B1*0.01667+B2*0.01667+B3*0.01667+B4*0.01667+B5*0.01667+B6*0.01667+B7*0.01667+B8*0.01667+B9*0.01667+B10*0.01667+B11*0.01667+B12*0.01667+B13*0.01667+B14*0.01667+B15*0.01667+B16*0.01667+B17*0.01667+B18*0.01667+B19*0.01667+B20*0.01667+B21*0.01667+B22*0.01667+B23*0.01667+B24*0.01667+B25*0.01667+B26*0.01667+B27*0.01667+B28*0.01667+B29*0.01667+B30*0.01667+B31*0.01667+B32*0.01667+B33*0.01667+B34*0.01667+B35*0.01667+B36*0.01667+B37*0.01667+B38*0.01667+B39*0.01667+B40*0.01667+B41*0.01667+B42*0.01667+B43*0.01667+B44*0.01667+B45*0.01667+B46*0.01667+B47*0.01667+B48*0.01667+B49*0.01667+B50*0.01667+B51*0.01667+B52*0.01667+B53*0.01667+B54*0.01667+B55*0.01667+B56*0.01667+B57*0.01667+B58*0.01667+B59*0.01667;
    B59=B58;
    B58=B57;
    B57=B56;
    B56=B55;
    B55=B54;
    B54=B53;
    B53=B52;
    B52=B51;
    B51=B50;
    B50=B48;
    B49=B49;
    B48=B47;
    B47=B46;
    B46=B45;
    B45=B44;
    B44=B43;
    B43=B42;
    B42=B41;
    B41=B40;
    B40=B39;
    B39=B38;
    B38=B37;
    B37=B36;
    B36=B35;
    B35=B34;
    B34=B33;
    B33=B32;
    B32=B31;
    B31=B30;
    B30=B29;
    B29=B28;
    B28=B27;
    B27=B26;
    B26=B25;
    B25=B24;
    B24=B23;
    B23=B22;
    B22=B21;
    B21=B20;
    B20=B19;
    B19=B18;
    B18=B17;
    B17=B16;
    B16=B15;
    B15=B14;
    B14=B13;
    B13=B12;
    B12=B11;
    B11=B10;
    B10=B9;
    B9=B8;
    B8=B7;
    B7=B6;
    B6=B5;
    B5=B4;
    B4=B3;
    B3=B2;
    B2=B1;
    B1=B0;

//sturen naar scherm
    pc.printf("%f\r\n",MOVAVG_B);

    //naar HID Scope
    scope.set(2,emg_valueB);        //ruwe data
    scope.set(3,filtered_emgB);     //filtered
    scope.send();
}


// *** MAIN ***
int main()
{
    pc.baud(115200);

    //bepaling van positie met triceps 1
    Ticker log_timerT1;

    arm_biquad_cascade_df1_init_f32(&notchT,1,notch_const,notch_states);
    arm_biquad_cascade_df1_init_f32(&lowpassT,1,lowpass_const,lowpass_states);
    arm_biquad_cascade_df1_init_f32(&highpassT,1,highpass_const,highpass_states);

    log_timerT1.attach(Triceps, 0.005);
    wait(5);
    log_timerT1.detach();

    //MOVAVG_T=MOVAVG_Positie1;

    // positie van batje met behulp van Triceps
    drempelwaardeT=0.1;

    if (MOVAVG_T >= drempelwaardeT) {
        yT1=1;

        //bepaling van positie met tricep 2
        Ticker log_timerT2;
        
        arm_biquad_cascade_df1_init_f32(&notchT,1,notch_const,notch_states);
        arm_biquad_cascade_df1_init_f32(&lowpassT,1,lowpass_const,lowpass_states);
        arm_biquad_cascade_df1_init_f32(&highpassT,1,highpass_const,highpass_states);
        
        log_timerT2.attach(Triceps, 0.005);
        wait(5);
        log_timerT2.detach();

        //MOVAVG_T=MOVAVG_Positie2;

        if (MOVAVG_T >= drempelwaardeT) {
            yT2=1;
        } else {
            yT2=0;
        }
    } else {
        yT1=0;
    }

    //*** INPUT MOTOR 2 ***
    positie=yT1+yT2;

    //controle positie op scherm
    if (positie==0) {
        pc.printf("Motor 2 gaat naar stand 0\n");
    } else {
        if (positie==1) {
            pc.printf("Motor 2 gaat naar stand 1\n");
        } else {
            if (positie==2) {
                pc.printf("Motor 1 beweegt met snelheid 2\n");
            }
        }
    }

    wait(5);

    Ticker log_timerB;

    arm_biquad_cascade_df1_init_f32(&notchB,1,notch_const,notch_states);
    arm_biquad_cascade_df1_init_f32(&lowpassB,1,lowpass_const,lowpass_states);
    arm_biquad_cascade_df1_init_f32(&highpassB,1,highpass_const,highpass_states);

    log_timerB.attach(Biceps,0.005);
    wait(5);//log_timerB wordt 2000 keer uitgevoerd
    log_timerB.detach();

    //bepaling van snelheidsstand met biceps

    drempelwaardeB1=2;
    drempelwaardeB2=5;
    drempelwaardeB3=10;

    if (MOVAVG_B >= drempelwaardeB1) {
        yB1=1;
        if (MOVAVG_B >= drempelwaardeB2) {
            yB2=1;
            if (MOVAVG_B >= drempelwaardeB3) {
                yB3=1;
            } else {
                yB3=0;
            }
        } else {
            yB2=0;
        }
    } else {
        yB1=0;
    }

    //*** INPUT MOTOR 1 ***
    snelheidsstand=yB1+yB2+yB3;

    //controle snelheidsstand op scherm
    if (snelheidsstand==0) {
        pc.printf("Motor 1 beweegt niet\n");
    } else {
        if (snelheidsstand==1) {
            pc.printf("Motor 1 beweegt met snelheid 1\n");
        } else {
            if (snelheidsstand==2) {
                pc.printf("Motor 1 beweegt met snelheid 2\n");
            } else {
                if (snelheidsstand==3) {
                    pc.printf("Motor 1 beweegt met snelheid 3\n");
                }
            }
        }
    }
}