Diff: main.cpp

Revision:

3:2f75bb307da3

Parent:

2:241b572207fb

Child:

4:61d5e7417c4c

--- a/main.cpp	Tue Oct 27 10:56:41 2015 +0000
+++ b/main.cpp	Tue Oct 27 13:22:39 2015 +0000
@@ -4,14 +4,29 @@
 #include <math.h>
 #include "HIDScope.h"
 #include <iostream>
+#include "QEI.h"
+
+// Define pin for motor control
+DigitalOut directionPin(D4);
+PwmOut PWM(D5);
+DigitalIn buttonccw(PTA4);
+DigitalIn buttoncw(PTC6);
+
+// define ticker
+
+
+Serial pc(USBTX, USBRX);
 
 Ticker HIDScope_timer;
 Ticker Filteren_timer;
+Ticker aansturen;
+
 HIDScope scope(2);
 
 // defining flags
 volatile bool Flag_filteren = false;
 volatile bool Flag_HIDScope = false;
+volatile bool left_movement = false;
 
 // making function flags.
 void Go_flag_filteren()
@@ -82,16 +97,24 @@
 
 double moving_average(double y, double &n1, double &n2, double &n3, double &n4, double &n5)
 {
- double average = (y + n1 + n2 +n3 + n4 + n5)/5;
- n5 = n4;
- n4 = n3;
- n3 = n2;
- n2 = n1;
- n1 = y;
- 
- return average;   
+    double average = (y + n1 + n2 +n3 + n4 + n5)/5;
+    n5 = n4;
+    n4 = n3;
+    n3 = n2;
+    n2 = n1;
+    n1 = y;
+
+    return average;
 }
-
+/*
+double threshold(double signal, const double lowtreshold, const double hightreshold)
+{
+    if (signal > hightreshold)
+        left = true;
+    else if (signal <lowtreshold)
+        left = false;
+}
+*/
 //Specifying filter coefficients highpass
 
 /* notch filter with 3 cascaded biquads*/
@@ -164,45 +187,48 @@
 
 
 
-
-
-
-
-
-
-
-
-
-
 double Filteren()
 {
     input = analog_emg_left.read();
     input = input-0.45; //FIRST SUBTRACT MEAN THEN FILTER
     //input_right = analog_emg_right.read();
-    
+
     // notch filter
     double y1 = biquad(input, notch_v11, notch_v21, notch1_a1, notch1_a2, notch1_b0, notch1_b1, notch1_b2);
     double y2 = biquad(y1, notch_v12, notch_v22, notch2_a1, notch2_a2, notch2_b0, notch2_b1, notch2_b2);
     double y3 = biquad(y2, notch_v13, notch_v23, notch3_a1, notch3_a2, notch3_b0, notch3_b1, notch3_b2);
-    
+
     //higpass filter
     double y4 = biquad(y3, high_v11, high_v21, highp1_a1, highp1_a2, highp1_b0, highp1_b1, highp1_b2);
     double y5 = biquad(y4, high_v12, high_v22, highp2_a1, highp2_a2, highp2_b0, highp2_b1, highp2_b2);
     double y6 = biquad(y5, high_v13, high_v23, highp3_a1, highp3_a2, highp3_b0, highp3_b1, highp3_b2);
 
-    //rectivier 
+    //rectivier
     double y7 = fabs(y6);
 
     //lowpas filter cascade
     double y8 = biquad(y7, low_v11, low_v21, lowp1_a1, lowp1_a2, lowp1_b0, lowp1_b1, lowp1_b2);
     double y9 = biquad(y8, low_v12, low_v22, lowp2_a1, lowp2_a2, lowp2_b0, lowp2_b1, lowp2_b2);
     double y10= biquad(y9, low_v13, low_v23, lowp3_a1, lowp3_a2, lowp3_b0, lowp3_b1, lowp3_b2);
-    
-    // moving average 
+
+    // moving average
     double filter_signal = moving_average(y10,n1,n2,n3,n4,n5);
 
+double high_threshold = 1200;
+double low_threshold = 500;
+
+    if (filter_signal > high_threshold) {
+        left_movement = true;
+    } else if (filter_signal < low_threshold) {
+        left_movement = false;
+    }
+
     return(filter_signal);
 }
+
+/*************************************************************motor control******************************************************************************************************/
+
+
 void HIDScope_kijken()
 {
     scope.set(0, input);
@@ -223,5 +249,9 @@
             Flag_HIDScope = false;
             HIDScope_kijken();
         }
+        if(left_movement) {
+            pc.printf("left = true /n");
+        }
     }
-}
\ No newline at end of file
+}
+