Diff: main.cpp

Revision:

2:5a5a54374f80

Parent:

1:082004527d2e

--- a/main.cpp	Thu Oct 11 07:56:45 2018 +0000
+++ b/main.cpp	Thu Oct 11 10:10:22 2018 +0000
@@ -7,9 +7,15 @@
 AnalogIn    emg1( A1 );
 
 Ticker      sample_timer;
-HIDScope    scope( 4 );
-DigitalOut  ledred(LED1);
+HIDScope    scope( 2 );
+DigitalOut  ledred(LED_RED);
 DigitalOut  ledgreen(LED_GREEN);
+DigitalOut  ledblue(LED_BLUE);
+InterruptIn calbutton(PTA4);
+
+
+const double Fs = 500;  //Sample frequency
+double cal_fact = 1;
 
 
 //------------Filter parameters----------------------
@@ -27,49 +33,43 @@
 const double a1HP = -1.9841702689557372;
 const double a2HP = 0.9844150813196749;
 //Notchfilter
-//const double b0NO = 1;
-//const double b1NO = 1;
-//const double b2NO = 1;
-//const double a1NO = 1;
-//const double a2NO = 1;
+const double b0NO = 0.7728616577547288;
+const double b1NO = -1.2505164308487402;
+const double b2NO = 0.7728616577547288;
+const double a1NO = -1.2505164308487402;
+const double a2NO = 0.5457233155094577;
 
 //--------------Filter------------
 BiQuad LP1( b0LP, b1LP, b2LP, a1LP, a2LP ); //Lowpass filter Biquad
 BiQuad HP2( b0HP, b1HP, b2HP, a1HP, a2HP ); //Highpass filter Biquad
-//BiQuad NO3( b0NO, b1NO, b2NO, a1NO, a2NO ); //Notch filter Biquad 
+BiQuad NO3( b0NO, b1NO, b2NO, a1NO, a2NO ); //Notch filter Biquad 
 BiQuadChain BiQuad_filter;
-float Signal;
-float Signal_filtered;
-float Signal_filtered_HP;
-float Signal_filtered_fabs;
+double Signal;
+double Signal_filtered;
+double Signal_filtered_HP;
+double Signal_filtered_fabs;
+double Signal_filtered_LP;
 
 
 
-/** Sample function
- * this function samples the emg and sends it to HIDScope
+/** Sample filter function
+ * this function samples the emg, filters it and sends it to HIDScope
  **/
 void sample_filter()
 {
     Signal = emg0;
     Signal_filtered_HP = HP2.step(Signal);
     Signal_filtered_fabs = fabs(Signal_filtered_HP);
-    Signal_filtered = LP1.step(Signal_filtered_fabs);
-
+    Signal_filtered_LP = LP1.step(Signal_filtered_fabs);
+    Signal_filtered = NO3.step(Signal_filtered_LP) * cal_fact;
  
-//Poging tot goed filter
-    //Signal = emg0-emg1;
-//    BiQuad_filter.add(&LP);
-//    Signal_filtered = BiQuad_filter.step(Signal);
-//    Signal_filtered = fabs(Signal_filtered);
-//    BiQuad_filter.add(&HP);
-//    Signal_filtered = BiQuad_filter.step(Signal_filtered);
-    
+
     //Signal_filtered = fabs(Signal_filtered);        // je wil alleen de absolute waardes
     /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
     scope.set(0, emg0.read() );
-    scope.set(1, emg1.read() );
-    scope.set(2, Signal );
-    scope.set(3, Signal_filtered);
+    //scope.set(1, emg1.read() );
+    //scope.set(2, Signal );
+    scope.set(1, Signal_filtered);
     /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels) 
     *  Ensure that enough channels are available (HIDScope scope( 2 ))
     *  Finally, send all channels to the PC at once */
@@ -78,16 +78,24 @@
     ledred = !ledred;
 }
 
-//----------Parameters voor filter----------
-//const float Fs = 200.0;
-//const float Fnyq = Fs / 2;
-//const int Nthorder = 4;
-//
-////---------Functie voor filter-----------
-//void filter()
-//{
-//   
-//}
+void calibratie()
+{
+    ledblue = 0;
+    cal_fact = 1;
+    double cal_max = 0;
+    
+     for(int i = 0; i < 5*Fs; i++)
+    {
+        sample_filter();
+        if(Signal_filtered > cal_max)
+        {
+            cal_max = Signal_filtered;
+        }
+        wait(1/(float)Fs);
+   }
+   cal_fact = 1.0/cal_max;
+   ledblue = 1;
+}
 
 int main()
 {   
@@ -95,13 +103,14 @@
     /**Attach the 'sample' function to the timer 'sample_timer'.
     * this ensures that 'sample' is executed every... 0.002 seconds = 500 Hz
     */
-    
-    sample_timer.attach(&sample_filter, 0.002);
+    ledblue = 1;
+    sample_timer.attach(&sample_filter, 1/(float)Fs);
+    calbutton.rise(&calibratie);
     
     /*empty loop, sample() is executed periodically*/
     while(1) {
         
-        if (Signal_filtered > 0.1)
+        if (Signal_filtered > 0.7)
         {   ledgreen = 0;
         }
         else {