Diff: main.cpp

Revision:

3:aacea1875b7a

Parent:

1:93a041e62d7e

diff -r 93a041e62d7e -r aacea1875b7a main.cpp
--- a/main.cpp	Mon Oct 08 12:59:46 2018 +0000
+++ b/main.cpp	Tue Nov 06 11:14:06 2018 +0000
@@ -1,56 +1,127 @@
-// Continuously sweep the servo through it's full range
 #include "mbed.h"
 #include "Servo.h"
+#include "BiQuad.h"
  
-Servo myservo(D7);
-InterruptIn button(PTA4);
-//InterruptIn button1(PTC6);
-//DigitalOut myledgreen(LED_GREEN);
-//DigitalOut myledred(LED_RED);
+Servo myservo(D6);
+
+AnalogIn    emgS(A0);                                                           // EMG Servo spier
+DigitalIn   CalButton(PTA4);                                                    // Button used for gaining zero and max
+DigitalIn   zeromax(PTC6);                                                      // Button used for switching between zero and max
+
+DigitalOut  ledB(LED_BLUE);                                                     // Informative LED for gaining zero and max
+DigitalOut  ledR(LED_RED);
+DigitalOut  ledG(LED_GREEN);                                                    // Informative LED for gaining zero and max
+
+Ticker      emgSampleTicker;                                                    // Ticker for sample frequency 
+
+ int R = 200;                                                                   // Number of points for movag third emg
+ double C[200];                                                                 // Vector for storing data of third emg
+ int k = 0;                                                                     // Counter for configuration:
+ double Svector[200];                                                           // Vector for Swaarde configuration
+ double Swaarde[2];                                                             // Vector for storage of max and zero of servo emg
+ int x = 0;                                                                     // Variable for switching between zero and max
+ double movagS;                                                                 // Moving Average mean value of servo spier                                                       
+ float thresholdS = 10;                                                         
+
+//Highpassfilter                        Fc = 10 Hz;, Q = 0.5, Fs = 500 Hz
+const double b0HP = 0.8851221317817073;
+const double b1HP = -1.7702442635634146;
+const double b2HP = 0.8851221317817073;
+const double a1HP =  -1.7632371847263784;
+const double a2HP = 0.777251342400451;
+//Notchfilter                           Fc = 50 Hz, Q = 10, Fs = 500 Hz
+const double b0NO = 0.9714498065192796;
+const double b1NO = -1.5718388053127037;
+const double b2NO = 0.9714498065192796;
+const double a1NO = -1.5718388053127037;
+const double a2NO = 0.9428996130385592;
+
+//BiQuad LPS( b0LP, b1LP, b2LP, a1LP, a2LP ); //Lowpass filter Biquad
+BiQuad HPS( b0HP, b1HP, b2HP, a1HP, a2HP ); //Highpass filter Biquad
+BiQuad NOS( b0NO, b1NO, b2NO, a1NO, a2NO ); //Notch filter Biquad 
+
+
+void emgSample() {
+    
+    double emgNOFilteredS = NOS.step(emgS.read());                              // Filtered NO value of EMG signal servo spier                
+    double emgHPFilteredS = HPS.step(emgNOFilteredS);                           // Filtered HP value of EMG signal servo spier
+    double emgabsS = fabs(emgHPFilteredS);                                      // Absolute value of EMG signal servo spier
+    
+    for(int i = R-1; i >= 0; i--){                                              // For-loop used for moving average
+        if (i == 0) {
+            C[i] = emgabsS;
+            }
+         else {
+             C[i] = C[i-1];
+             }   
+    }
+    double sumS = 0;
+    for (int n = 0; n < R-1; n++) {                                             // Summation of array
+        sumS = sumS + C[n];
+    }
+    movagS = sumS/R;  
+    
+     if (CalButton==0 & k<200) {                                                 // Loop used for gaining max and zero value
+        Svector[k] = movagS;
+        
+        if (x==0){
+        ledB = !ledB;                                                           // SPIER NIET AANSPANNEN BIJ BLAUW
+        }                                                                       // SPIER WEL AANSPANNEN BIJ ROOD
+        else if (x==1){
+        ledR = !ledR;
+        }
+        k++;
+    }    
+    else if (k==200) {                                                          // End of the loop, used for calculation value
+        double sumX = 0;
+            for (int n = 0; n < 199; n++) {                                     
+            sumX = sumX + Svector[n];
+            } 
+        Swaarde[x] = sumX/200;                                                  // Value of zero for Servo spier
+        k++;
+        ledB = 1;
+        ledR = 1;
+        ledG = !ledG;
+    }
+    else if (k == 201 && zeromax ==0) {                                           // Loop used for switching between zero and max
+        ledG = !ledG;
+        k = 0;
+        if (x==0) {
+            x++;
+        }
+        else if (x==1) {
+            x=0;
+        }
+    }
+    if (x==1)                                                                   // Determining threshold using zero and max
+    {
+        thresholdS = Swaarde[0]+(Swaarde[1]-Swaarde[0])*(0.25f);
+    }
+}    
 
 
 
 
 int main() {
+    
+ledB = 1;
+ledG = 1;
+ledR = 1;
+emgSampleTicker.attach( &emgSample, 0.002 );                                    // Ticker for EMG function
+    
     while(1) {
         
-      // Met een switch
-      
-    switch(button)
-    { 
-        case 0:
-        myservo = 0.5;
-        wait(0.01);
-        break;
-        
-        default :
-        myservo = 0.0;
-        wait(0.01);
-        
+        if (movagS > thresholdS)
+        {   myservo = 0.5;
+            ledB = 0;
+            wait(0.01);
         }
-        
-        
-        
-    // Met een if-loop
-  /*        
-    if (button == 0){
-        myledgreen = 0;
-        myledred = 1;
-    
-             myservo = 50/100.0;
-             wait(0.01);
-    
-        myledgreen = 0;
-        myledred = 1;
-        }  
-    else if (button1 == 0){
-        myservo = 0.001;
-        myledgreen = 1;
-        myledred = 0;
-        }
-        */
-        
+        else {
+            myservo = 0.0;
+            wait(0.01);
+            ledB = 1;
+            }
+    }
 }
-       }
     
         
\ No newline at end of file