File content as of revision 1:6a8b45298e54:

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

HIDScope scope(2);//is dit 4 voor 2 spieren? en hoe zit het met scope.set?

AnalogIn emgB(PTB0);

//*** OBJECTS ***
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, MOVAVG_B;//moving average objects
int snelheidsstand;

//*** FILTERS ***
arm_biquad_casd_df1_inst_f32 lowpass;
//constants for 50Hz lowpass
float lowpass_const[] = {0.2928920553, 0.5857841107, 0.2928920554, -0, -0.17156822136};//{a0 a1 a2 -b1 -b2} van online calculator
float lowpass_states[4];

arm_biquad_casd_df1_inst_f32 highpass;
//constants for 10Hz highpass
float highpass_const[] = {0.8005910267, -1.6011820533, 0.8005910267, 1.5610153913, -0.6413487154};//{a0 a1 a2 -b1 -b2}
float highpass_states[4];

//*** CALIBRATIE ***

//

//*** BICEP EMG ***
void Biceps()
{
    uint16_t emg_valueB;
    float emg_value_f32B;


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


    //filteren
    arm_biquad_cascade_df1_f32(&highpass, &emg_value_f32B, &filtered_emgB, 1 );

    arm_biquad_cascade_df1_f32(&lowpass, &filtered_emgB, &filtered_emgB, 1 );

    filtered_emgB = fabs(filtered_emgB);


    //moving average
    B0=filtered_emgB;

    MOVAVG_B=B0*0.1+B1*0.1+B2*0.1+B3*0.1+B4*0.1+B5*0.1+B6*0.1+B7*0.1+B8*0.1+B9*0.1;

    B9=B8;
    B8=B7;
    B7=B6;
    B6=B5;
    B5=B4;
    B4=B3;
    B3=B2;
    B2=B1;
    B1=B0;

    //naar scherm sturen
    scope.set(0,emg_valueB);
    scope.set(1,MOVAVG_B);
    scope.send();

// *** TRICEP EMG ***
void Triceps(){

// *** MAIN ***
    int main() {

        Ticker log_timer;
        arm_biquad_cascade_df1_init_f32(&lowpass,1,lowpass_const,lowpass_states);
        arm_biquad_cascade_df1_init_f32(&highpass,1,highpass_const,highpass_states);

        log_timer.attach(Biceps, 0.005);
        while(1) {}

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

        //controle
        snelheidsstand=yB1+yB2+yB3;

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