Diff: main.cpp

Revision:

29:9010b7bdbfbb

Parent:

28:433d12c52913

Child:

30:14a9358758d1

--- a/main.cpp	Tue Oct 30 10:23:46 2018 +0000
+++ b/main.cpp	Wed Oct 31 13:40:00 2018 +0000
@@ -1,15 +1,22 @@
 #include "mbed.h"
 #include "HIDScope.h"
 #include "BiQuad.h"
+#include "QEI.h"
 
 HIDScope scope( 6 );
 Ticker sample_timer;
 
+
+Serial pc(USBTX,USBRX);
+
 // Inputs EMG
 AnalogIn emg0_in( A0 );
 AnalogIn emg1_in( A1 );
 AnalogIn emg2_in( A2 );
 
+InterruptIn but1(SW2);
+InterruptIn but2(SW3);
+
 DigitalOut led1(LED_BLUE);
 DigitalOut led2(LED_RED);
 DigitalOut led3(LED_GREEN);
@@ -62,6 +69,8 @@
 BiQuad L2( c1, c2, d0, d1, d2);
 BiQuad L3( c1, c2, d0, d1, d2);
 
+const float T=0.001f;
+
 // EMG
 const int sizeMovAg = 100; //Size of array over which the moving average (MovAg) is calculated
 double sum, sum1, sum2, sum3; //Variables used in calibration and MovAg to sum the elements in the array
@@ -100,7 +109,17 @@
     low0 = L1.step(absolute0); // Applying low pass filter
     low1 = L2.step(absolute1);
     low2 = L3.step(absolute2); 
-    
+}
+
+/*void EMG_filterd()
+{ 
+    low0();
+    low1();
+    low2();
+}*/
+
+void MovAg()
+{    
     for (int i = sizeMovAg-1; i>=0; i--) {
 //For statement to make an array of the last datapoints of the filtered signal
 StoreArray0[i] = StoreArray0[i-1]; //Shifts the i'th element one place to the right
@@ -131,16 +150,7 @@
     scope.send(); 
 }
 
-int main() 
-{  
-    sample_timer.attach( &filtering, 0.002);
-
-    while(1) {}
-}
-
-
-
-void Switching() //Void to switch between signals to calibrate
+void switch_to_calibrate() //Void to switch between signals to calibrate
 {
     g++;
     if (g == 0) 
@@ -169,7 +179,7 @@
         led3=0;
     }
 }
-void Calibration(void) //Void to calibrate the signals, depends on value g. While calibrating, maximal contraction is required
+void calibrate(void) //Void to calibrate the signals, depends on value g. While calibrating, maximal contraction is required
 
 {
     switch (g) {
@@ -182,7 +192,7 @@
                 wait(0.001f);
             }
             Mean0 = sum/sizeCali; //Calculates the mean of the signal
-            Threshold0 = Mean0/2; //Determines the threshold (factor 0.5)
+            Threshold0 = Mean0; //Determines the threshold (factor 0.5)
             break;
     }
         case 1: { //Case one, calibrate EMG signal of left biceps
@@ -193,7 +203,7 @@
                 wait(0.001f);
             }
             Mean1 = sum/sizeCali;
-            Threshold1 = Mean1/2;
+            Threshold1 = Mean1;
             break;
     }
         case 2: { //case two, calibrate EMG signal of calf
@@ -204,7 +214,7 @@
                 wait(0.001f);
             }   
             Mean2 = sum/sizeCali;
-            Threshold2 = Mean2/2;
+            Threshold2 = Mean2;
             break;
     }
         case 3: { //Sets calibration value to 1; robot can be set to Home position
@@ -216,4 +226,35 @@
 break;
 }
 }
-}
\ No newline at end of file
+}
+
+
+int main() 
+{  
+    sample_timer.attach( &filtering, 0.002);
+    pc.baud(115200); // setting baudrate 
+
+    while(true) 
+    {
+        led1 = 0;
+        led2 = 1;
+        led3 = 1; 
+     Filter_tick.attach(&filtering,T);                         //EMG signals filtered every T sec.
+     MovAg_tick.attach(&MovAg,T);                                //Moving average calculation every T sec.
+     //ticker.attach(&dv_des_calculate_qref,T);                 //v_des determined every T
+              
+       // HIDScope_tick.attach(&HIDScope_sample,T);             //EMG signals raw + filtered to HIDScope every T sec.
+        
+        but1.rise(switch_to_calibrate);                      //Switch state of calibration (which muscle)
+        wait(0.2f);                                             //Wait to avoid bouncing of button
+        but2.rise(calibrate);                                //Calibrate threshold for 3 muscles
+        wait(0.2f);                                             //Wait to avoid bouncing of button
+    
+        pc.printf("g is %i\n\r",g);
+        pc.printf("Average0 = %f , Average1 = %f, Average2 = %f \n\r",Average0, Average1, Average2);
+        pc.printf("Thresh0 = %f , Thresh1 = %f, Thresh2 = %f \n\r",Threshold0, Threshold1, Threshold2);
+        //wait(2.0f);   
+    }
+}
+
+