aanpassing met notch

Dependencies:   Encoder HIDScope MODSERIAL mbed

Fork of TEST by Daan

Revision:
3:ef4d1423db92
Parent:
2:902c38ab4f5a
Child:
4:81448a7baf64
--- a/main.cpp	Wed Oct 08 15:16:17 2014 +0000
+++ b/main.cpp	Thu Oct 09 13:12:39 2014 +0000
@@ -10,44 +10,58 @@
 /* -Marjolein Thijssen                 */
 /***************************************/
 #include "mbed.h"
-#include "MODSERIAL.h"
-#include "encoder.h"
 #include "HIDScope.h"
+#include "arm_math.h"
+
 //Define objects
-#define A1 0.807313721035803
-#define A2 -0.263720649619515
-#define B1 -1.721118336253692
-#define B2 0.721118336253692
+AnalogIn    emg0(PTB0); //Biceps
+AnalogIn    emg1(PTB1); //Triceps
 HIDScope scope(2);
 
-AnalogIn    emgbc(PTB0); //Onderste bordje biceps meting
-AnalogIn    emgtr(PTB1); //Tweede bordje triceps meting
-//DigitalOut motordirA(PTD3);
-//DigitalOut motordirB(PTD1);
-//Encoder motor1(PTD0,PTC9);
-//Encoder motor2(PTD5,PTC8);
-
-MODSERIAL   pc(USBTX,USBRX);
+arm_biquad_casd_df1_inst_f32 lowpass;
+//constants for 5Hz lowpass
+float lowpass_const[] = {0.02008337 , 0.04016673 , 0.02008337 , 1.56101808 , -0.64135154};
+//state values
+float lowpass_states[4];
+arm_biquad_casd_df1_inst_f32 highpass;
+//constants for 5Hz highpass
+float highpass_const[] = {0.802041575714419, -1.604083151428837, 0.802041575714419, 1.0, -1.564503986101199, 0.643662316756476};
+//state values
+float highpass_states[4];
 
-float filter(float);
+void looper()
+{
+    /*variable to store value in*/    
+    uint16_t emg_value;
+    float filtered_emg;
+    float emg_value_f32;
+    /*put raw emg value both in red and in emg_value*/
+    emg_value = emg0.read_u16(); // read direct ADC result, converted to 16 bit integer (0..2^16 = 0..65536 = 0..3.3V)
+    emg_value_f32 = emg0.read();
 
-int main() {
-    float x,y;
-    while(true) {
-        x = (emgbc.read()-0.5)*2;
-        y=filter(x);
-        scope.set(0,x);
-        scope.set(1,y);
-        scope.send();
-        }
+    //process emg
+    arm_biquad_cascade_df1_f32(&highpass, &emg_value_f32, &filtered_emg, 1 );
+    filtered_emg = fabs(filtered_emg);
+    arm_biquad_cascade_df1_f32(&lowpass, &filtered_emg, &filtered_emg, 1 );
+    
+    /*send value to PC. */
+    scope.set(0,emg_value);     //uint value
+    scope.set(1,filtered_emg);  //processed float
+    scope.send();
+
 }
 
-float filter(float x) {
-    static float y,x1,y1,x2,y2;
-    y = A1*y1+A2*y2+x+B1*x1+B2*x2;
-    x2=x1;
-    x1=x;
-    y2=y1;
-    y1=y;
-    return y;
-}
+int main()
+{
+    Ticker log_timer;
+   //set up filters. Use external array for constants
+    arm_biquad_cascade_df1_init_f32(&lowpass,1 , lowpass_const, lowpass_states);
+    arm_biquad_cascade_df1_init_f32(&highpass,1 ,highpass_const,highpass_states);
+    
+    log_timer.attach(looper, 0.005);
+    while(1) //Loop
+    {
+      /*Empty!*/
+      /*Everything is handled by the interrupt routine now!*/
+    }
+}
\ No newline at end of file