File content as of revision 2:0a8622662f6d:

//Voor het toevoegen van een button:
#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
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
double emg1_filtered, emg2_filtered, emg3_filtered, emg4_filtered;
volatile 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);
    
    
    //Send data to HIDScope
    //scope.set(0,emg1_filtered ); ONLY FOR VISUALIZATION
    //scope.set(1,emg2_filtered);
    //scope.set(2,emg3_filtered);
    //scope.set(3,emg4_filtered);
    //scope.send();
          
    }
    

//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 p_t;
    double threshold1;
    double threshold2;
    double threshold3;
    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;
                    }
                }
            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;
                    }
                }
            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;
                    }
                }
            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(emg3_filtered>temp_highest_emg3)
                    {
                    temp_highest_emg3= emg3_filtered;
                    }
                }
    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);
    
    p_t = 0.8;
    threshold1 = temp_highest_emg1*p_t;
    threshold2 = temp_highest_emg2*p_t; 
    threshold3 = temp_highest_emg3*p_t;
    threshold4 = temp_highest_emg4*p_t;   
}

void threshold_check(){
     
    //Check if emg_filtered has reached their threshold
    bool bicepsR;
    bool tricepsR;
    bool bicepsL;
    bool tricepsL;
    
    //EMG1 threshold check
    if(emg1_filtered>threshold1){
        bicepsR = true;
        }
    else{
        bicepsR= false;
        }
    //EMG2 threshold check
    if(emg2_filtered>threshold2){
        tricepsR = true;
        }
    else{
        tricepsR= false;
        }
    //EMG3 threshold check
     if(emg3_filtered>threshold3){
        bicepsL = true;
        }
    else{
        bicepsL= false;
        }
    //EMG4 threshold check
     if(emg4_filtered>threshold4){
        tricepsL = true;
        }
    else{
        tricepsL= false;
        }
        
    pc.printf("Biceps Right = %d", bicepsR);
    pc.printf("Triceps Right = %d",tricepsR);
    pc.printf("Biceps Left = %d", bicepsL);
    pc.printf("Triceps Left = %d", tricepsL);
    
}

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();
                 
       
        if(timer_calibration<55){
            sample_ticker.attach(&emgsample, ts);
            CalibrationEMG();
            }
        else{
            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.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.start();
        led1 = 1;
        led2 = 0;
        led3 = 0;
        pc.printf("De kleur is paars ok");
        //sample_ticker.attach(&threshold_check, ts);
        //sample_ticker.attach(&emgsample, ts);
        
        stateChanged = false;
      }
          
      // State transition logic: naar CLICK (button1), naar MOTORS_OFF(button2) anders naar MOVEMENT
        if (!button1)
        {        
        currentState = CLICK;
        stateChanged = true; 
        }
        else if (!button2)
        {        
        currentState = MOTORS_OFF  ;
        stateChanged = true;
        }
        else if (t>300)
        {        
        t.stop();
        t.reset();
        currentState = HOMING  ;
        stateChanged = true;
        }
        else
        {        
        currentState = MOVEMENT  ;
        stateChanged = true;
        }
        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()
{
    pc.baud(115200);
    while (true)
    {
    led1 = 1;
    led2 = 1;
    led3 = 1;
    ProcessStateMachine();
    
    }
    
}