File content as of revision 4:dfe39188f2b2:

#include "mbed.h"
#include "QEI.h"
#include "HIDScope.h"
//#include "MODSERIAL.h"
#include "BiQuad.h"
#include "math.h"

#define IGNORECOUNT 100

PwmOut pwmpin1(D6);
PwmOut pwmpin2(D5);
AnalogIn potmeter1(A5);
AnalogIn potmeter2(A4);
DigitalIn button1(D2);
DigitalIn button2(D3);
DigitalOut directionpin1(D4);
DigitalOut directionpin2(D7);
QEI motor1(D13,D12,NC, 32);
QEI motor2(D11,D10,NC, 32);

//Define objects
AnalogIn    emg0( A0 );     // EMG at A0
BiQuad emg0bq1(0.8848578, -1.7697156, 0.8848578, -1.7539023, 0.7855289); // highpass at 30Hz  Q at around 1
BiQuad emg0bq2(0.0773021,0.1546042,0.0773021,-1.3098283,0.6190368); // lowpass at 130 Hz Q at around .6
BiQuad emg0bq3(0.9556457,-1.81774618, 0.955645, -1.817746, 0.9112914); // 50 Hz notch Q at 4.5
BiQuadChain emg0bqc; // merged chain of three filters

AnalogIn    emg1( A1 );     // EMG at A1
BiQuad emg1bq1(0.8848578, -1.7697156, 0.8848578, -1.7539023, 0.7855289); // highpass at 30Hz  Q at around 1
BiQuad emg1bq2(0.0773021,0.1546042,0.0773021,-1.3098283,0.6190368); // lowpass at 130 Hz Q at around .6
BiQuad emg1bq3(0.9556457,-1.81774618, 0.955645, -1.817746, 0.9112914); // 50 Hz notch Q at 4.5
BiQuadChain emg1bqc; // merged chain of three filters

AnalogIn    emg2( A2 );     // EMG at A2
BiQuad emg2bq1(0.8848578, -1.7697156, 0.8848578, -1.7539023, 0.7855289); // highpass at 30Hz  Q at around 1
BiQuad emg2bq2(0.0773021,0.1546042,0.0773021,-1.3098283,0.6190368); // lowpass at 130 Hz Q at around .6
BiQuad emg2bq3(0.9556457,-1.81774618, 0.955645, -1.817746, 0.9112914); // 50 Hz notch Q at 4.5
BiQuadChain emg2bqc; // merged chain of three filters

DigitalIn kill_switch(SW2); //position has to be changed
DigitalIn next_switch(SW3); //name and position should be replaced

enum position_states{PositionCalibration, EmgCalibration, Movement, KILL};
enum emg_states{EMG0, EMG1, EMG2};
position_states CurrentState;
emg_states CalibrationState;

Ticker sample_timer;
Ticker MotorsTicker;
Timer timer;

//for testing purposes
DigitalOut ledred(LED_RED);
DigitalOut ledgreen(LED_GREEN);
DigitalOut ledblue(LED_BLUE);
//MODSERIAL pc(USBTX, USBRX);
HIDScope scope(6);

bool    emg0Bool    = 0;        // I don't know if these NEED to be global, but when I tried to put them in they wouldn't work...
int     emg0Ignore  = 0;
bool    emg1Bool    = 0;
int     emg1Ignore  = 0;
bool    emg2Bool    = 0;
int     emg2Ignore  = 0;
float   Calibration0;
float   Calibration1;
float   Calibration2;

double  input = 0;              // raw input
double  filtHigh = 0;           // filtered after highpass
double  filtlow = 0;            // filtered after lowpass
double  filtNotch = 0;          // filtered after notch
double  emg0filteredAbs;

float threshold0;
float threshold1;
float threshold2;

volatile float pwm_value1 = 0.0;
volatile float pwm_value2 = 0.0;

void positionCalibration() {
    while(!button1) {
        directionpin1 = true;
        pwm_value1 = 0.7f;
    }
    pwm_value1 = 0.0f;
    while(!button2) {
        directionpin2 = true;
        pwm_value2 = 0.6f;
    }
    pwm_value2 = 0.0f;
    
    if(!next_switch) {
        CurrentState = EmgCalibration;
    }
}

void CalibrateEMG0(){          // calibrates only one muscle activities as testing concluded that it was unnessecary to do this for all muscles.
    ledred = !ledred;
    int C = 500;           // half a second at 1000Hz
    double A0=0, A1=0, A2=0, A3=0, A4=0;
    double emg0FiAbs;
    while (C > 0){
        emg0FiAbs = fabs( emg1bqc.step(emg1.read()));
        if (C==500){                        //first instance make all values the first in case this is the highest
            A0=A1=A2=A3=A4=emg0FiAbs;
        }
        else if(emg0FiAbs > A0){            // if there is a higher value change the inputs to be the highest 5
            A4=A3;
            A3=A2;
            A2=A1;
            A1=A0;
            A0=emg0FiAbs;
        }
        C--;
        wait(0.001f);
    }
    Calibration0 = (A0+A1+A2+A3+A4)/5*0.4;  // average of the 5 highest values x 0,4 to create the threshold
    ledred = !ledred;
}  

void CalibrateEMG1(){          // calibrates only one muscle activities as testing concluded that it was unnessecary to do this for all muscles.
    ledgreen = !ledgreen;
    int C = 500;           // half a second at 1000Hz
    double A0=0, A1=0, A2=0, A3=0, A4=0;
    double emg1FiAbs;
    while (C > 0){
        emg1FiAbs = fabs( emg1bqc.step(emg1.read()));
        if (C==500){                        //first instance make all values the first in case this is the highest
            A0=A1=A2=A3=A4=emg1FiAbs;
        }
        else if(emg1FiAbs > A0){            // if there is a higher value change the inputs to be the highest 5
            A4=A3;
            A3=A2;
            A2=A1;
            A1=A0;
            A0=emg1FiAbs;
        }
        C--;
        wait(0.001f);
    }
    Calibration1 = (A0+A1+A2+A3+A4)/5*0.4;  // average of the 5 highest values x 0,4 to create the threshold
    ledgreen = !ledgreen;
}  

void CalibrateEMG2(){          // calibrates only one muscle activities as testing concluded that it was unnessecary to do this for all muscles.
    ledblue = !ledblue;
    int C = 500;           // half a second at 1000Hz
    double A0=0, A1=0, A2=0, A3=0, A4=0;
    double emg2FiAbs;
    while (C > 0){
        emg2FiAbs = fabs( emg2bqc.step(emg2.read()));
        if (C==500){                        //first instance make all values the first in case this is the highest
            A0=A1=A2=A3=A4=emg2FiAbs;
        }
        else if(emg2FiAbs > A0){            // if there is a higher value change the inputs to be the highest 5
            A4=A3;
            A3=A2;
            A2=A1;
            A1=A0;
            A0=emg2FiAbs;
        }
        C--;
        wait(0.001f);
    }
    Calibration2 = (A0+A1+A2+A3+A4)/5*0.4;  // average of the 5 highest values x 0,4 to create the threshold
    ledblue = !ledblue;
}    

bool emg0Filter(void){  
    double emg0filteredAbs = fabs( emg0bqc.step(emg0.read())); // Filter and make absolute, 
    /* this is the threshhold */ 
    if (emg0filteredAbs > Calibration0) {  // when above threshold  set bool to 1, here can the parameters be changed using global variables
        emg0Bool = 1;
        emg0Ignore = IGNORECOUNT;   // here is the counter reset to catch sudden jolts
        }
    else if (emg0Ignore < 0){ // if the ignore-counter is down to zero, set the bool back to 0
         emg0Bool = 0;
        }
    else {
        emg0Ignore--;      // else decrease counter by one each time has passed without threshold being met
        }  
    return emg0Bool;
}

bool emg1Filter(void){  
    double emg1filteredAbs = fabs( emg1bqc.step(emg1.read())); // Filter and make absolute
    /* this is the threshhold */ 
    if (emg1filteredAbs > Calibration1) {  // when above threshold  set bool to 1 here can the parameters be changed using global variables
        emg1Bool = true;
        emg1Ignore = IGNORECOUNT;   // here is the counter increased ( at 1000 Hz, this is 0.1 sec)
        }
    else if (emg1Ignore < 0){ // if the ignore-counter is down to zero, set the bool back to 0
         emg1Bool = false;
        }
    else {
        emg1Ignore--;      // else decrease counter by one each time has passed without threshold being met
        }
    return emg1Bool;
}

bool emg2Filter(void){  
    double emg2filteredAbs = fabs( emg2bqc.step(emg2.read())); // Filter and make absolute
    /* this is the threshhold */ 
    if (emg2filteredAbs > Calibration2) {  // when above threshold  set bool to 1 here can the parameters be changed using global variables
        emg2Bool = true;
        emg2Ignore = IGNORECOUNT;   // here is the counter increased ( at 1000 Hz, this is 0.1 sec)
        }
    else if (emg2Ignore < 0){ // if the ignore-counter is down to zero, set the bool back to 0
        emg2Bool = false;
        }
    else {
        emg2Ignore--;      // else decrease counter by one each time has passed without threshold being met
        }

    return emg2Bool;
}

void sample() {  
    emg0Filter();
    emg1Filter();
    emg2Filter();
    
    scope.set(0, Calibration0);
    scope.set(1, Calibration1);
    scope.set(2, Calibration2);
    scope.set(3, emg0Bool);
    scope.set(4, emg1Bool);
    scope.set(5, emg2Bool);
    scope.send();
}

void emgCalibration() {
    emg0bqc.add( &emg0bq1 ).add( &emg0bq2 ).add ( &emg0bq3 );       // combining biquad chains is done in main, before the ticker, so it happens only once.
    emg1bqc.add( &emg1bq1 ).add( &emg1bq2 ).add ( &emg1bq3 );
    emg2bqc.add( &emg2bq1 ).add( &emg2bq2 ).add ( &emg2bq3 );
    
    bool u = true;
    CalibrationState = EMG0;
    while (u){                      // !!! this is a place holder for the calibration!!! 
        switch(CalibrationState) {
            case EMG0:
            if(!next_switch) {   
                CalibrateEMG0(); // calibration function
                CalibrationState = EMG1;
            }
            break;
            case EMG1:
            if(!next_switch) {   
                CalibrateEMG1(); // calibration function
                CalibrationState = EMG2;
            }
            break;
            case EMG2:
            if(!next_switch) {      
                CalibrateEMG2(); // calibration function
                CurrentState = Movement;
                sample_timer.attach(&sample, 0.001);    //ticker at 1000Hz
                ledred = false;     // to indicate filter is working
                ledgreen = false;
                ledblue = false;
                u = false;
            }
            break;
        }
    }     
}



void movement() {
    
}

void move_motors() {
    pwmpin1 = pwm_value1;
    pwmpin2 = pwm_value2;
}

int main()
{
//    pc.baud(115200);
//    pc.printf(" ** program reset **\n\r");
    pwmpin1.period_us(60);
    pwmpin2.period_us(60);
    directionpin1 = true;
    directionpin2 = true;
    
    ledred = true;
    ledgreen = true;
    ledblue = true;
        
    MotorsTicker.attach(&move_motors, 0.02f); //ticker at 50Hz

    CurrentState = PositionCalibration;
//    pc.printf("current state = PositionCalibration\n\r");
    
    while (true) {
        switch(CurrentState) {
            case PositionCalibration:
            ledgreen = false;
            positionCalibration();
            if (!kill_switch) {
                CurrentState = KILL;   
//                pc.printf("current state = KILL\n\r"); 
            }
            break;
                
            case EmgCalibration:
            ledgreen = true;
            ledblue = false;
            emgCalibration();
            //emg1Calibration();
            //emg2Calibration();
            
            if (!kill_switch) {
                CurrentState = KILL;   
//                    pc.printf("current state = KILL\n\r");  
            }
            break;
            
            case Movement:
            ledred = true;
            ledgreen = false;
            ledblue = false;
            movement();
            
            if (!kill_switch) {
                CurrentState = KILL; 
//                pc.printf("current state = KILL\n\r");     
            }
            break;
            
            case KILL:
            bool u = true;
            ledgreen = true;
            ledblue = true;
            ledred = false;
            
            pwm_value1 = 0;
            pwm_value2 = 0;
            
            timer.start();
            if (timer.read_ms()> 2000) {
                timer.stop();
                timer.reset();
                while(u) {
                    if (!kill_switch) {
                        timer.start();
                        u = false;
                        ledred = true;
                        CurrentState = PositionCalibration;
//                         pc.printf("current state = PositionCalibration\n\r");
                        wait(1.0f); 
                    }
                }
            }
            break;  
        }
        wait(0.2f);         
    }
}