Diff: main.cpp

Revision:

15:d3a849ab3697

Parent:

12:768048d7f742

Child:

16:24e992616cf6

--- a/main.cpp	Thu Sep 11 07:19:37 2014 +0000
+++ b/main.cpp	Wed Sep 24 08:26:36 2014 +0000
@@ -7,6 +7,26 @@
 Ticker log_timer;
 MODSERIAL pc(USBTX,USBRX);
 
+typedef struct second_order_constants
+{
+    float b[3];
+    float a[3];
+} second_order_constants_t;
+
+typedef struct second_order_values
+{
+    float x_1,x_2;
+    float y_1,y_2;
+} second_order_values_t;
+
+//constants
+second_order_constants_t highpass= {{0.97803048, -1.95606096,  0.97803048},{1, -1.95557824,  0.95654368}};
+//type for values
+second_order_values_t highpass_values;
+
+float second_order(float x, second_order_constants_t constants, second_order_values_t &values);
+
+
 /** Looper function
 * functions used for Ticker and Timeout should be of type void <name>(void)
 * i.e. no input arguments, no output arguments.
@@ -24,12 +44,14 @@
 {
     /*variable to store value in*/    
     uint16_t emg_value;
+    float filtered_emg;
     /*put raw emg value both in red and in emg_value*/
     red.write(emg0.read());      // read float value (0..1 = 0..3.3V)
     emg_value = emg0.read_u16(); // read direct ADC result, converted to 16 bit integer (0..2^16 = 0..65536 = 0..3.3V)
+    filtered_emg = second_order((float)emg_value,highpass, highpass_values);
     /*send value to PC. Line below is used to prevent buffer overrun */
     if(pc.rxBufferGetSize(0)-pc.rxBufferGetCount() > 30)
-        pc.printf("%u\n",emg_value);
+        pc.printf("%u\t%f\n",emg_value, filtered_emg);
     /**When not using the LED, the above could also have been done this way:
     * pc.printf("%u\n", emg0.read_u16());
     */
@@ -51,4 +73,18 @@
       /*Empty!*/
       /*Everything is handled by the interrupt routine now!*/
     }
+}
+
+float second_order(float x, second_order_constants_t constants, second_order_values_t &values)
+{
+    float y = 0;
+    float b_terms, a_terms;
+    b_terms = (constants.b[0]*x) + (constants.b[1]*values.x_1) + (constants.b[2]*values.x_2);
+    a_terms = (constants.a[1]*values.y_1) + (constants.a[2]*values.y_2);
+    y=(1./constants.a[0])* (b_terms-a_terms);
+    values.x_2 = values.x_1;
+    values.x_1 = x;
+    values.y_2 = values.y_1;
+    values.y_1 = y;
+    return y;
 }
\ No newline at end of file