Diff: main.cpp

Revision:

9:ba7294959988

Parent:

8:5f13198a8e49

Child:

10:4af887af3a47

diff -r 5f13198a8e49 -r ba7294959988 main.cpp
--- a/main.cpp	Tue Oct 27 11:56:15 2015 +0000
+++ b/main.cpp	Tue Oct 27 22:00:46 2015 +0000
@@ -4,11 +4,14 @@
 #include "MODSERIAL.h"
 #include "QEI.h"
 //Define objects
-AnalogIn    emg(A0); //Analog of EMG input
-Ticker      sample_timer;
-Ticker      motor_timer;
+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(2);        // Instantize a 2-channel HIDScope object
+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);
@@ -27,15 +30,25 @@
 biquadFilter filterlow1(1.9556, 0.9565, 0.9780, 1.9561, 0.9780);
 biquadFilter notch(-1.1978e-16, 0.9561, 0.9780, -1.1978e-16, 0.9780);
 biquadFilter filterlow2(-1.9645, 0.9651, 1.5515e-4, 3.1030e-4, 1.5515e-4);
-double emg_value;
-double signalpart1;
-double signalpart2;
-double signalpart3;
-double signalpart4;
-double signalfinal;
-double onoffsignal;
-double maxcal=0;        
-bool calyes=0;
+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
 
@@ -54,24 +67,41 @@
 
 /* 
  */
-void filter(){
+void filter1(){
         if(calyes==1){
-        emg_value = emg.read();//read the emg value from the elektrodes
-        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
-        onoffsignal=signalfinal/maxcal;//divide the emg signal by the max EMG to calibrate the signal per person
-        scope.set(0,emg_value);//set emg signal to scope in channel 1
-        scope.set(1,onoffsignal);//set filtered signal to scope in channel 2
+        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
+        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.send();//send the signals to the scope
         //pc.printf("emg signal %f, filtered signal %f \n",emg_value,onoffsignal);
 }
 }
-void checkmotor(){//check the normalized signal and set actions if a threshold is passed
+
+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
+    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(onoffsignal >= 0.5){
+    if(onoffsignal1 >= 0.05){
                  led1.write(0);
                  led2.write(1);
             while(n1 == 1){
@@ -82,7 +112,7 @@
                 n1=0;
                   }
              }
-    else if(onoffsignal<=0.25){
+    else if(onoffsignal1<=0.03){
              led1.write(1);
              led2.write(0);
              
@@ -95,10 +125,36 @@
         }
 }
 
+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<5000;n++){//read for 5000 samples as calibration
-            emg_value = emg.read();//read the emg value from the electrodes
+        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
@@ -112,18 +168,69 @@
         calyes=1;
     }  
     }
+    */
 int main()
 {
     pc.baud(115200);
     led1.write(1);
     led2.write(1);
-   
-        sample_timer.attach(&filter, 0.002);//continously execute the EMG reader and filter
-        motor_timer.attach(&checkmotor, 0.002);//continously execute the motor controller
-        cal_timer.attach(&calibri, 0.002);//ticker to check if motor is being calibrated
+    led3.write(1);
+    speed1.write(0);
+    speed2.write(0);
+        sample_timer1.attach(&filter1, 0.002);//continously execute the EMG reader and filter
+        sample_timer2.attach(&filter2, 0.002);//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){
- //random while loop to keep system going                                                 
+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;
+             }
+            }
+    
+        }
 }     
 }
\ No newline at end of file