File content as of revision 0:3710031b2621:

#include "mbed.h"
#include "BiQuad.h"
#include "HIDScope.h"
#include <sys/time.h>

class Timer
{
private:
    struct timeval start_t;
public:
    double start() { gettimeofday(&start_t, NULL); }
    double get_ms() {
       struct timeval now;
       gettimeofday(&now, NULL);
       return (now.tv_usec-start_t.tv_usec)/(double)1000.0 +
              (now.tv_sec-start_t.tv_sec)*(double)1000.0;
    }
    double get_ms_reset() {
      double res = get_ms();
      reset();
      return res;
    }
    Timer() { start(); }
};

Timer t();
double used_ms;
Serial pc(USBTX,USBRX);// serial connection to pc

DigitalOut led_red(LED_RED);
DigitalOut led_green(LED_RED);
DigitalIn buttonR(D2);//rigth button on biorobotics shield
DigitalIn buttonL(D3);//rigth button on biorobotics shield

DigitalOut motor1_direction(D4);// draairichting motor 1 (1 is CW encoder als je daar op kijkt en CW shaft als je daar op kijkt)
PwmOut motor1_speed_control(D5);//aanstuursnelheid motor 1

DigitalOut motor2_direction(D7);// draairichting motor 2 (1 is CW encoder als je daar op kijkt en CW shaft als je daar op kijkt)
PwmOut motor2_speed_control(D6);//aanstuursnelheid motor 2

enum States {failure, wait, calib_emg, operational, demo};
enum Operations {rest, forward, backward, up, down};

States current_state;
Operations movement = rest;

float max1 = 0.3; //initial threshold value for emg signals, changes during calibration
float max2 = 0.3;

Ticker      sample_timer;
Ticker      loop_timer;

HIDScope    scope( 3 );

AnalogIn raw_emg1_input(A0);//input for first emg signal 1, for the modes
AnalogIn raw_emg2_input(A1);//input for first emg signal 2, for the strength

volatile float raw_filteredsignal1;//the first filtered emg signal 1
volatile float raw_filteredsignal2;//the first filtered emg signal 2

volatile float filteredsignal1;//the first filtered emg signal 1
volatile float filteredsignal2;//the first filtered emg signal 2

bool state_changed = false;

measureall(){ // changes all variables according in sync with the rest of the code
    
    emg1_input = emg1_input.read();
    emg2_input = emg2_input.read();
    filterall();
    filteredsignal1 = raw_filteredsignal1;
    filteredsignal2 = raw_filteredsignal2;
    //Reading of motor
    
    
    
}


void filterall()
{
    //Highpass Biquad 5 Hz 
static BiQuad HighPass(0.95653708,  -1.91307417, 0.95653708, -1.91118480, 0.91496354);
float high1 = HighPass.step(raw_emg1_input);
float high2 = HighPass.step(raw_emg2_input);
    // Rectify the signal(absolute value)
float abs1 = fabs(high1);
float abs2 = fabs(high2);
    //Lowpass Biquad 10 Hz
static BiQuad LowPass(0.00362164,  0.00724327, 0.00362164, -1.82267251, 0.83715906);
float low1 = LowPass.step(abs1);
float low2 = LowPass.step(abs2);

raw_filteredsignal1 = low1;
raw_filteredsignal2 = low2;

}

void scopedata()
{
    scope.set(0,emg1_input); // 
    scope.set(1,filteredsignal1); //    
    scope.set(2,filteredsignal2); //
    scope.send(); // send info to HIDScope server
}

void loop_function() {
    measureall();    
    switch(current_state) {
    case failure:
        do_state_failure(); // a separate function for what happens in each state
        break;
    case calib_emg:
        do_state_calib_emg();
        break;
    case operational:
        do_state_operational();
    break;
    case wait;
        do_state_wait();
        break;  
}
motor_controller();
scopedata(); // Outputs data to the computer
}

do_state_failure(){    
    //al motor movement to zero!
    wait(1000);    
    };

do_state_calib_emg(){    
    if (state_changed==true) {
        state_changed = false;        
    }
    if(filteredsignal1 > max1){//calibrate to a new maximum
       max1 = filteredsignal1;
       }
    if(filteredsignal2 > max2){//calibrate to a new maximum
       max2 = filteredsignal2;
       }
       
    if (filteredsignal1 > (0.75*max1) && filteredsignal2 > (0.75*max2)) {
        current_state = operational;
        used_ms = t.get_ms_reset();
        state_changed = true;
    }    
}

do_state_operational(){
     if (state_changed==true) {
        state_changed = false;        
    }
    
    switch(States) {// a separate function for what happens in each state
    case rest:
        if (filteredsignal2 > (0.6*max))) {// 
        current_state = wait;
        state_changed = true;
    }              
        if( t.get_ms() > 1000 && filteredsignal1 > (0.6max))
        {
            States = forward;
            used_ms = t.get_ms_reset();
            }
        break;
    case forward:
        do_forward();
         if( t.get_ms() > 1000 && filteredsignal1 > (0.6max))
        {
            States = backward;
            used_ms = t.get_ms_reset();
            }
        break;
    case backward:
        do_backward();
         if( t.get_ms() > 1000 && filteredsignal1 > (0.6max))
        {
            States = up;
            used_ms = t.get_ms_reset();
            }
        break;    
    case up:
        do_up();
         if( t.get_ms() > 1000 && filteredsignal1 > (0.6max))
        {
            States = wait;
            used_ms = t.get_ms_reset();
            }
        break;
    case down:
        do_down();
        if( t.get_ms() > 1000 && filteredsignal1 > (0.6max))
        {
            States = wait;
            used_ms = t.get_ms_reset();
            }
        break;    
    }
       
    
}

do_state_wait(){
    if (state_changed==true) {
        state_changed = false;        
    }   
       
    if (filteredsignal1 > (0.75*max1) && filteredsignal2 > (0.75*max2) {
        current_state = operational;
        state_changed = true;
    } 
}



int main()
{      
       
    loop_timer.attach(&loop_function, 0.002);
    
    pc.baud(115200);
    
    while (true) {         
    if(buttonR == true){
        current_state = failure; 
        }
    }
}