File content as of revision 8:ec3ea0623620:

// EMG, RKI, PID, MOTOR
#include "mbed.h"
#include <iostream>
#include "BiQuad.h"
#include "BiQuadchains_zelfbeun.h"
#include "MODSERIAL.h"

MODSERIAL pc(USBTX, USBRX);

DigitalOut gpo(D0);

DigitalIn button2(SW3);
DigitalIn button1(SW2); //or SW2

DigitalOut led1(LED_GREEN);
DigitalOut led2(LED_RED);
DigitalOut led3(LED_BLUE);

//EMG tickers, these tickers are called in the mainscript with fsample 500Hz, also sends to HIDscope with same fsample
Ticker sample_ticker; //ticker for filtering pref. with 1000Hz, define in tick.attach
Ticker threshold_check_ticker;
Timer t; //timer try out for Astrid
Timer timer_calibration; //timer for EMG calibration



//Input system
AnalogIn emg1(A0); //right biceps
AnalogIn emg2(A1); //right triceps
AnalogIn emg3(A2); //left biceps
AnalogIn emg4(A3); //left triceps

//Filtered EMG signals from the end of the chains
volatile double emg1_filtered, emg2_filtered, emg3_filtered, emg4_filtered;
int i = 0;

void emgsample()
{
    //All EMG signal through Highpass
    double emgread1 = emg1.read();
    double emgread2 = emg2.read();
    double emgread3 = emg3.read();
    double emgread4 = emg4.read();

    double emg1_highpassed = highp1.step(emgread1);
    double emg2_highpassed = highp2.step(emgread2);
    double emg3_highpassed = highp3.step(emgread3);
    double emg4_highpassed = highp4.step(emgread4);

    //All EMG highpassed through Notch
    double emg1_notched = notch1.step(emg1_highpassed);
    double emg2_notched = notch2.step(emg2_highpassed);
    double emg3_notched = notch3.step(emg3_highpassed);
    double emg4_notched = notch4.step(emg4_highpassed);

    //All EMG notched rectify
    double emg1_abs = abs(emg1_notched);
    double emg2_abs = abs(emg2_notched);
    double emg3_abs = abs(emg3_notched);
    double emg4_abs = abs(emg4_notched);

    //All EMG abs into lowpass
    emg1_filtered = lowp1.step(emg1_abs);
    emg2_filtered = lowp2.step(emg2_abs);
    emg3_filtered = lowp3.step(emg3_abs);
    emg4_filtered = lowp4.step(emg4_abs);


}


//Define doubles for calibration and ticker
double ts = 0.001; //tijdsstap
double calibration_time = 55; //time EMG calibration should take

volatile double temp_highest_emg1 = 0; //highest detected value right biceps
volatile double temp_highest_emg2 = 0;
volatile double temp_highest_emg3 = 0;
volatile double temp_highest_emg4 = 0;

//Doubles for calculation threshold
double biceps_p_t = 0.4; //set threshold at percentage of highest value
double triceps_p_t = 0.5; //set threshold at percentage of highest value
volatile double threshold1;
volatile double threshold2;
volatile double threshold3;
volatile double threshold4;

void CalibrationEMG()
{
    //static float samples = calibration_time/ts;
    while(timer_calibration<55) {
        if(timer_calibration>0 && timer_calibration<10) {
            led1=!led1;
            if(emg1_filtered>temp_highest_emg1) {
                temp_highest_emg1= emg1_filtered;
                pc.printf("Temp1 = %f \r\n",temp_highest_emg1);
            }
        }
        if(timer_calibration>10 && timer_calibration<15) {
            led1=0;
            led2=0;
            led3=0;
        }
        if(timer_calibration>15 && timer_calibration<25) {
            led2=!led2;
            if(emg2_filtered>temp_highest_emg2) {
                temp_highest_emg2= emg2_filtered;
                pc.printf("Temp2 = %f \r\n",temp_highest_emg2);
            }
        }
        if(timer_calibration>25 && timer_calibration<30) {
            led1=0;
            led2=0;
            led3=0;
        }
        if(timer_calibration>30 && timer_calibration<40) {
            led3=!led3;
            if(emg3_filtered>temp_highest_emg3) {
                temp_highest_emg3= emg3_filtered;
                pc.printf("Temp3 = %f \r\n",temp_highest_emg3);
            }
        }
        if(timer_calibration>40 && timer_calibration<45) {
            led1=0;
            led2=0;
            led3=0;
        }
        if(timer_calibration>45 && timer_calibration<55) {
            led2=!led2;
            led3=!led3;
            if(emg4_filtered>temp_highest_emg4) {
                temp_highest_emg4= emg4_filtered;
                pc.printf("Temp4 = %f \r\n",temp_highest_emg4);
            }
        }
        led1=1;
        led2=1;
        led3=1;


    }

    pc.printf("Highest value right biceps= %f \r\n", temp_highest_emg1);
    pc.printf("Highest value right triceps= %f \r\n", temp_highest_emg2);
    pc.printf("Highest value left biceps= %f \r\n", temp_highest_emg3);
    pc.printf("Highest value left triceps= %f \r\n", temp_highest_emg4);


    threshold1 = temp_highest_emg1*biceps_p_t;  //Right Biceps
    threshold2 = temp_highest_emg2*triceps_p_t; //Right Triceps
    threshold3 = temp_highest_emg3*biceps_p_t;  //Left Biceps
    threshold4 = temp_highest_emg4*triceps_p_t; //Left Triceps
}

//Check if emg_filtered has reached their threshold
int bicepsR;
int tricepsR;
int bicepsL;
int tricepsL;

void threshold_check()
{

    //EMG1 threshold check
    if(emg1_filtered>threshold1) {
        bicepsR = 1;
    } else {
        bicepsR= 0;
    }
    //EMG2 threshold check
    if(emg2_filtered>threshold2) {
        tricepsR = 1;
    } else {
        tricepsR= 0;
    }
    //EMG3 threshold check
    if(emg3_filtered>threshold3) {
        bicepsL = 1;
    } else {
        bicepsL= 0;
    }
    //EMG4 threshold check
    if(emg4_filtered>threshold4) {
        tricepsL = 1;
    } else {
        tricepsL= 0;
    }

    /*
    pc.printf("Biceps Right = %i", bicepsR);
    pc.printf("Triceps Right = %i",tricepsR);
    pc.printf("Biceps Left = %i", bicepsL);
    pc.printf("Triceps Left = %i", tricepsL);
    */


}


//Activate ticker for Movement state, filtering and Threshold checking
void movement_ticker_activator()
{
    sample_ticker.attach(&emgsample, ts);
    threshold_check_ticker.attach(&threshold_check, ts);
}
void movement_ticker_deactivator()
{
    sample_ticker.detach();
    threshold_check_ticker.detach();
}

enum states {MOTORS_OFF,CALIBRATION,HOMING,DEMO,MOVEMENT,CLICK};
states currentState = MOTORS_OFF; //Chosen startingposition for states
bool stateChanged = true; // Make sure the initialization of first state is executed

void ProcessStateMachine(void)
{
    switch (currentState) {
        case MOTORS_OFF:
            // Actions
            if (stateChanged) {
                // state initialization: rood
                led1 = 1;
                led2 = 0;
                led3 = 1;
                wait (1);
                stateChanged = false;
            }

            // State transition logic: Als button 1 word ingedrukt --> calibratie, anders motor uithouden
            if (!button1) {
                currentState = CALIBRATION;
                stateChanged = true;
            } else if (!button2) {
                currentState = HOMING  ;
                stateChanged = true;
            } else {
                currentState = MOTORS_OFF;
                stateChanged = true;
            }

            break;

        case CALIBRATION:
            // Actions
            if (stateChanged) {
                // state initialization: oranje
                temp_highest_emg1 = 0; //highest detected value right biceps
                temp_highest_emg2 = 0;
                temp_highest_emg3 = 0;
                temp_highest_emg4 = 0;

                timer_calibration.reset();
                timer_calibration.start();

                sample_ticker.attach(&emgsample, ts);
                CalibrationEMG();
                sample_ticker.detach();
                timer_calibration.stop();


                stateChanged = false;
            }

            // State transition logic: automatisch terug naar motors off.

            currentState = MOTORS_OFF;
            stateChanged = true;
            break;

        case HOMING:
            // Actions
            if (stateChanged) {
                // state initialization: green
                t.reset();
                t.start();
                led1 = 0;
                led2 = 1;
                led3 = 1;
                wait (1);

                stateChanged = false;
            }

            // State transition logic: naar DEMO (button1), naar MOVEMENT(button2)
            if (!button1) {
                currentState = DEMO;
                stateChanged = true;
            } else if (!button2) {
                currentState = MOVEMENT  ;
                stateChanged = true;
            } else if (t>300) {
                t.stop();
                t.reset();
                currentState = MOTORS_OFF  ;
                stateChanged = true;
            } else {
                currentState = HOMING  ;
                stateChanged = true;
            }
            break;

        case DEMO:
            // Actions
            if (stateChanged) {
                // state initialization: light blue
                led1 = 0;
                led2 = 1;
                led3 = 0;
                wait (1);

                stateChanged = false;
            }

            // State transition logic: automatisch terug naar HOMING
            currentState = HOMING;
            stateChanged = true;
            break;

        case MOVEMENT:
            // Actions
            if (stateChanged) {
                // state initialization: purple
                //t.reset();
                //t.start();

                led1 = 1;
                led2 = 0;
                led3 = 0;
                wait(2);

                movement_ticker_activator();

                led1 = 0;
                led2 = 0;
                led3 = 0;
                wait(2);


                stateChanged = false;
            }

            // State transition logic: naar CLICK (button1), naar MOTORS_OFF(button2) anders naar MOVEMENT
            if (!button1) {
                movement_ticker_deactivator();
                currentState = CLICK;
                stateChanged = true;
            } else if (!button2) {
                movement_ticker_deactivator();
                currentState = MOTORS_OFF  ;
                stateChanged = true;
            } else if (bicepsR==0 && tricepsR==0 && bicepsL==0 && tricepsL==0) { //this check if person is idle for more than 300seconds
                t.start();
            } else if  (bicepsR==1 || tricepsR==1 || bicepsL==1 || tricepsL==1) {
                t.stop();
                t.reset();
            }

            if(t>20) {
                movement_ticker_deactivator();
                t.stop();
                t.reset();
                currentState = HOMING  ;
                stateChanged = true;
            }
            // here ends the idle checking mode
            else {
                //For every muscle a different colour if threshold is passed
                if(bicepsR==1) {
                    led1 = 0;
                    led2 = 1;
                    led3 = 1;
                } else if (bicepsR==0 && tricepsR==0 && bicepsL==0 && tricepsL==0 ) {
                    led1 = 1;
                    led2 = 1;
                    led3 = 1;
                }
                if(tricepsR==1) {
                    led1 = 1;
                    led2 = 0;
                    led3 = 1;
                } else if (bicepsR==0 && tricepsR==0 && bicepsL==0 && tricepsL==0 ) {
                    led1 = 1;
                    led2 = 1;
                    led3 = 1;
                }
                if(bicepsL==1) {
                    led1 = 1;
                    led2 = 1;
                    led3 = 0;
                } else if (bicepsR==0 && tricepsR==0 && bicepsL==0 && tricepsL==0 ) {
                    led1 = 1;
                    led2 = 1;
                    led3 = 1;
                }
                if(tricepsL==1) {
                    led1 = 1;
                    led2 = 0;
                    led3 = 0;
                } else if (bicepsR==0 && tricepsR==0 && bicepsL==0 && tricepsL==0 ) {
                    led1 = 1;
                    led2 = 1;
                    led3 = 1;
                }
                currentState = MOVEMENT  ;
                stateChanged = false;
            }

            break;

        case CLICK:
            // Actions
            if (stateChanged) {
                // state initialization: blue
                led1 = 1;
                led2 = 1;
                led3 = 0;
                wait (1);

                stateChanged = false;
            }

            // State transition logic: automatisch terug naar MOVEMENT.

            currentState = MOVEMENT;
            stateChanged = true;
            break;

    }
}

int main()
{
    //BiQuad Chain add
    highp1.add( &highp1_1 ).add( &highp1_2 );
    notch1.add( &notch1_1 ).add( &notch1_2 );
    lowp1.add( &lowp1_1 ).add(&lowp1_2);

    highp2.add( &highp2_1 ).add( &highp2_2 );
    notch2.add( &notch2_1 ).add( &notch2_2 );
    lowp2.add( &lowp2_1 ).add(&lowp2_2);

    highp3.add( &highp3_1 ).add( &highp3_2 );
    notch3.add( &notch3_1 ).add( &notch3_2 );
    lowp3.add( &lowp3_1 ).add(&lowp3_2);

    highp4.add( &highp4_1 ).add( &highp4_2 );
    notch4.add( &notch4_1 ).add( &notch4_2 );
    lowp4.add( &lowp4_1 ).add(&lowp4_2);

    pc.baud(115200);
    led1 = 1;
    led2 = 1;
    led3 = 1;

    while (true) {
        ProcessStateMachine();

    }

}