File content as of revision 11:8196434745b4:

#include "mbed.h"
#include "HIDScope.h"
#include "biquadFilter.h"        // Require the HIDScope library
#include "MODSERIAL.h"
#include "QEI.h"
//Define objects
AnalogIn    emg1(A0); //Analog of EMG input
AnalogIn    emg2(A1);
Ticker      sample_timer1;
Ticker      sample_timer2;
Ticker      motor_timer1;
Ticker      motor_timer2;
Ticker      cal_timer;
HIDScope    scope(4);        // Instantize a 2-channel HIDScope object
DigitalIn button1(PTA4);//test button for starting motor 1
DigitalOut led1(LED_RED);
DigitalOut led2(LED_BLUE);
DigitalOut led3(LED_GREEN);
MODSERIAL pc(USBTX,USBRX);
// motor objects
QEI motor1(D13,D12,NC, 624);//encoder for motor 1
QEI motor2(D11,D10,NC, 624);//encoder for motor 2
DigitalOut direction1(D7);//direction input for motor 1
DigitalOut direction2(D4);//direction input for motor 2
PwmOut speed1(D6);//speed input for motor 1
PwmOut speed2(D5);//speed input for motor 2

/*The biquad filters required to transform the EMG signal into an usable signal*/
biquadFilter filterhigh1(-1.4542, 0.5741, 0.7571, -1.5142, 0.7571);
biquadFilter filterlow1(0.4395, 0.2059, 0.4114, 0.8227, 0.4114);
biquadFilter notch(-1.0958, 0.9723, 0.9862, -1.0958, 0.9862);
biquadFilter filterlow2(-1.4542, 0.5741, 0.0300, 0.0599, 0.0300);
double emg_value1;
double signalpart11;
double signalpart12;
double signalpart13;
double signalpart14;
double signalfinal1;
double onoffsignal1;

double emg_value2;
double signalpart21;
double signalpart22;
double signalpart23;
double signalpart24;
double signalfinal2;
double onoffsignal2;


double maxcal=1;        
bool calyes=1;
bool motor1_dir=0;//set the direction of motor 1
bool motor2_dir = 0;//set the direction of motor 1

float cycle = 0.3;//define the speed of the motor
   bool motor1_on = 1;//set the on variable of motor 1
   bool motor2_on =1;//set the on variable of motor 2
   int n1=1;//numeric conditions to determine if the speed needs to be increased
   int n2=1;

void changedirmotor1(){
        motor1_dir = !motor1_dir;//code for changing direction of motor 1
        }
void changedirmotor2(){
        motor2_dir = !motor2_dir;//code for changing direction of motor 2
        }        

/* 
 */
void filter1(){
        if(calyes==1){
        emg_value1 = emg1.read();//read the emg value from the elektrodes
        signalpart11 = notch.step(emg_value1);//Highpass filter for removing offset and artifacts
        signalpart12 = filterhigh1.step(signalpart11);//rectify the filtered signal
        signalpart13 = abs(signalpart12);//low pass filter to envelope the emg
        signalpart14 = filterlow1.step(signalpart13);//notch filter to remove 50Hz signal
        signalfinal1 = filterlow2.step(signalpart14);//2nd low pass filter to envelope the emg
        onoffsignal1=signalfinal1/maxcal;//divide the emg signal by the max EMG to calibrate the signal per person
        //pc.printf("the emg 1 signal is: %f \n",onoffsignal1); 
        emg_value2 = emg2.read();//read the emg value from the elektrodes
        signalpart21 = notch.step(emg_value2);//Highpass filter for removing offset and artifacts
        signalpart22 = filterhigh1.step(signalpart21);//rectify the filtered signal
        signalpart23 = abs(signalpart22);//low pass filter to envelope the emg
        signalpart24 = filterlow1.step(signalpart23);//notch filter to remove 50Hz signal
        signalfinal2 = filterlow2.step(signalpart24);//2nd low pass filter to envelope the emg
        onoffsignal2=signalfinal2/maxcal;//divide the emg signal by the max EMG to calibrate the signal per person
        
        scope.set(0,emg_value1);//set emg signal to scope in channel 1
        scope.set(1,onoffsignal1);//set filtered signal to scope in channel 2
        scope.set(2,emg_value2);//set emg signal to scope in channel 1
        scope.set(3,onoffsignal2);//set filtered signal to scope in channel 2
        scope.send();//send the signals to the scope
        //pc.printf("the emg 2 signal is: %f \n",onoffsignal2); 

        //pc.printf("emg signal %f, filtered signal %f \n",emg_value,onoffsignal);
}
}
/*
void filter2(){
        if(calyes==1){
        emg_value2 = emg2.read();//read the emg value from the elektrodes
        signalpart21 = notch.step(emg_value2);//Highpass filter for removing offset and artifacts
        signalpart22 = filterhigh1.step(signalpart21);//rectify the filtered signal
        signalpart23 = abs(signalpart22);//low pass filter to envelope the emg
        signalpart24 = filterlow1.step(signalpart23);//notch filter to remove 50Hz signal
        signalfinal2 = filterlow2.step(signalpart24);//2nd low pass filter to envelope the emg
        onoffsignal2=signalfinal2/maxcal;//divide the emg signal by the max EMG to calibrate the signal per person
        scope.set(2,emg_value2);//set emg signal to scope in channel 1
        scope.set(3,onoffsignal2);//set filtered signal to scope in channel 2
        pc.printf("the emg 2 signal is: %f \n",onoffsignal2); 
    scope.send();//send the signals to the scope
        //pc.printf("emg signal %f, filtered signal %f \n",emg_value,onoffsignal);
}
}*/

/*
void checkmotor1(){//check the normalized signal and set actions if a threshold is passed not needed in ticker move to main function
    if(calyes==1){
    if(onoffsignal1 >= 0.05){
                 led1.write(0);
                 led2.write(1);
            while(n1 == 1){
                changedirmotor1();
                speed1.write(cycle);//write speed only on first run through the loop
                direction1.write(motor1_dir);//turn motor CCW or CW 
                  
                n1=0;
                  }
             }
    else if(onoffsignal1<=0.03){
             led1.write(1);
             led2.write(0);
             
            while(n1==0){//check if the first run was done
                 speed1.write(0);//if so set speed to 0 and reset the run counter
                 n1=1;
             }
            }
    
        }
}

void checkmotor2(){//check the normalized signal and set actions if a threshold is passed
    if(calyes==1){
    if(onoffsignal2 >= 0.05){
                 led1.write(0);
                 led2.write(1);
            while(n2 == 1){
                changedirmotor2();
                speed2.write(cycle);//write speed only on first run through the loop
                direction2.write(motor2_dir);//turn motor CCW or CW 
                  
                n2=0;
                  }
             }
    else if(onoffsignal2<=0.03){
             led1.write(1);
             led2.write(0);
             
            while(n2==0){//check if the first run was done
                 speed2.write(0);//if so set speed to 0 and reset the run counter
                 n2=1;
             }
            }
    
        }
}

void calibri(){//calibration function
    if(button1.read()==false){
        for(int n =0; n<100000;n++){//read for 5000 samples as calibration
            emg_value = emg1.read();//read the emg value from the electrodes
            signalpart1 = notch.step(emg_value);//Highpass filter for removing offset and artifacts
            signalpart2 = filterhigh1.step(signalpart1);//rectify the filtered signal
            signalpart3 = abs(signalpart2);//low pass filter to envelope the emg
            signalpart4 = filterlow1.step(signalpart3);//notch filter to remove 50Hz signal
            signalfinal = filterlow2.step(signalpart4);//2nd low pass filter to envelope the emg
            double signalmeasure = signalfinal;
            if (signalmeasure > maxcal){//determine what the highest reachable emg signal is
                maxcal = signalmeasure;
                }
            }
        calyes=1;
    }  
    }
    */
int main()
{
    pc.baud(115200);
    led1.write(1);
    led2.write(1);
    led3.write(1);
    speed1.write(0);
    speed2.write(0);
        sample_timer1.attach(filter1, 0.003125);//continously execute the EMG reader and filter
//        sample_timer2.attach(&filter2, 0.005);//continously execute the EMG reader and filter
        //motor_timer1.attach(&checkmotor1, 0.002);//continously execute the motor controller
        //motor_timer2.attach(&checkmotor2, 0.002);//continously execute the motor controller
      //  cal_timer.attach(&calibri, 0.002);//ticker to check if motor is being calibrated
        

while(1){
if(calyes==1){
    if(onoffsignal1 >= 0.05){
                 led1.write(0);
                 led2.write(1);
            while(n1 == 1){
                changedirmotor1();
                speed1.write(cycle);//write speed only on first run through the loop
                direction1.write(motor1_dir);//turn motor CCW or CW 
                  
                n1=0;
                  }
             }
    else if(onoffsignal1<=0.03){
             led1.write(1);
             led2.write(0);
             
            while(n1==0){//check if the first run was done
                 speed1.write(0);//if so set speed to 0 and reset the run counter
                 n1=1;
             }
            }
    
        }
        
        if(calyes==1){
    if(onoffsignal2 >= 0.05){
                 led1.write(0);
                 led2.write(1);
            while(n2 == 1){
                changedirmotor2();
                speed2.write(cycle);//write speed only on first run through the loop
                direction2.write(motor2_dir);//turn motor CCW or CW 
                  
                n2=0;
                  }
             }
    else if(onoffsignal2<=0.03){
             led1.write(1);
             led2.write(0);
             
            while(n2==0){//check if the first run was done
                 speed2.write(0);//if so set speed to 0 and reset the run counter
                 n2=1;
             }
            }
    
        }
}     
}