Diff: main.cpp

Revision:

9:a1890454e5a7

Parent:

8:54fd13779d00

Child:

10:9319e872c752

--- a/main.cpp	Wed Oct 22 10:08:24 2014 +0000
+++ b/main.cpp	Thu Oct 23 07:40:07 2014 +0000
@@ -5,19 +5,23 @@
 
 MODSERIAL pc(USBTX,USBRX);
 
-HIDScope scope(4);//is dit 4 voor 2 spieren? en hoe zit het met scope.set?
+HIDScope scope(4);
 
 AnalogIn emgB(PTB1);//biceps
 AnalogIn emgT(PTB2); // tricep
 
 //*** OBJECTS ***
 //bicep
+uint16_t emg_valueB;
+float emg_value_f32B;
 float filtered_emgB;
 float drempelwaardeB1, drempelwaardeB2, drempelwaardeB3;//B1=snelheidsstand 1, B2=snelheidsstand 2, B3=snelheidsstand 3
 int yB1, yB2, yB3;
 float B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, MOVAVG_B;//moving average objects
 int snelheidsstand;
 //tricep
+uint16_t emg_valueT;
+float emg_value_f32T;
 float filtered_emgT;
 float drempelwaardeT1, drempelwaardeT2;//T1=positie 1, T2=positie 2
 int yT1, yT2;
@@ -26,12 +30,14 @@
 
 
 //*** FILTERS ***
-arm_biquad_casd_df1_inst_f32 lowpass;
+arm_biquad_casd_df1_inst_f32 lowpassT;
+arm_biquad_casd_df1_inst_f32 lowpassB;
 //constants for 50Hz lowpass
 float lowpass_const[] = {0.2928920553, 0.5857841107, 0.2928920554, -0, -0.17156822136};//{a0 a1 a2 -b1 -b2} van online calculator
 float lowpass_states[4];
 
-arm_biquad_casd_df1_inst_f32 highpass;
+arm_biquad_casd_df1_inst_f32 highpassT;
+arm_biquad_casd_df1_inst_f32 highpassB;
 //constants for 10Hz highpass
 float highpass_const[] = {0.8005910267, -1.6011820533, 0.8005910267, 1.5610153913, -0.6413487154};//{a0 a1 a2 -b1 -b2}
 float highpass_states[4];
@@ -71,22 +77,55 @@
 //}
 //******************************
 
-//*** BICEP EMG ***
+
+
+
+// *** TRICEPS en BICEPS EMG ***
+void Triceps()
+{
+    //Triceps lezen
+    emg_valueT = emgT.read_u16();
+    emg_value_f32T = emgT.read();
+
+    //Triceps filteren
+    arm_biquad_cascade_df1_f32(&highpassT, &emg_value_f32T, &filtered_emgT, 1 );
+    arm_biquad_cascade_df1_f32(&lowpassT, &filtered_emgT, &filtered_emgT, 1 );
+    filtered_emgT = fabs(filtered_emgT);
+
+    pc.printf("%u\n",filtered_emgT);
+
+    //Triceps moving average
+    T0=filtered_emgT;
+    MOVAVG_T=T0*0.1+T1*0.1+T2*0.1+T3*0.1+T4*0.1+T5*0.1+T6*0.1+T7*0.1+T8*0.1+T9*0.1;
+    T9=T8;
+    T8=T7;
+    T7=T6;
+    T6=T5;
+    T5=T4;
+    T4=T3;
+    T3=T2;
+    T2=T1;
+    T1=T0;
+    scope.set(0,emg_valueT);        //ruwe data
+    scope.set(1,filtered_emgT);     //filtered
+    scope.send();
+}
+
 void Biceps()
 {
-    uint16_t emg_valueB;
-    float emg_value_f32B;
-
-    //lezen
+    //Biceps lezen
     emg_valueB = emgB.read_u16(); // read direct ADC result, converted to 16 bit integer (0..2^16 = 0..65536 = 0..3.3V)
     emg_value_f32B = emgB.read();
 
-    //filteren
-    arm_biquad_cascade_df1_f32(&highpass, &emg_value_f32B, &filtered_emgB, 1 );
-    arm_biquad_cascade_df1_f32(&lowpass, &filtered_emgB, &filtered_emgB, 1 );
+    //Biceps filteren
+    arm_biquad_cascade_df1_f32(&highpassB, &emg_value_f32B, &filtered_emgB, 1 );
+    arm_biquad_cascade_df1_f32(&lowpassB, &filtered_emgB, &filtered_emgB, 1 );
     filtered_emgB = fabs(filtered_emgB);
 
-    //moving average
+    pc.printf("%f\r\n",filtered_emgB);
+
+
+    //Biceps moving average
     B0=filtered_emgB;
     MOVAVG_B=B0*0.1+B1*0.1+B2*0.1+B3*0.1+B4*0.1+B5*0.1+B6*0.1+B7*0.1+B8*0.1+B9*0.1;
     B9=B8;
@@ -105,80 +144,31 @@
     scope.send();
 }
 
-// *** TRICEP EMG ***
-void Triceps()
-{
-    uint16_t emg_valueT;
-    float emg_value_f32T;
-
-    //lezen
-    emg_valueT = emgT.read_u16();
-    emg_value_f32T = emgT.read();
-
-    //filteren
-    arm_biquad_cascade_df1_f32(&highpass, &emg_value_f32T, &filtered_emgT, 1 );
-    filtered_emgT = fabs(filtered_emgT);
-    arm_biquad_cascade_df1_f32(&lowpass, &filtered_emgT, &filtered_emgT, 1 );
-
-    //moving average
-    T0=filtered_emgT;
-    MOVAVG_T=T0*0.1+T1*0.1+T2*0.1+T3*0.1+T4*0.1+T5*0.1+T6*0.1+T7*0.1+T8*0.1+T9*0.1;
-    T9=T8;
-    T8=T7;
-    T7=T6;
-    T6=T5;
-    T5=T4;
-    T4=T3;
-    T3=T2;
-    T2=T1;
-    T1=T0;
-    
-    uint16_t emg_valueB;
-    float emg_value_f32B;
-
-    //lezen
-    emg_valueB = emgB.read_u16(); // read direct ADC result, converted to 16 bit integer (0..2^16 = 0..65536 = 0..3.3V)
-    emg_value_f32B = emgB.read();
-
-    //filteren
-    arm_biquad_cascade_df1_f32(&highpass, &emg_value_f32B, &filtered_emgB, 1 );
-    arm_biquad_cascade_df1_f32(&lowpass, &filtered_emgB, &filtered_emgB, 1 );
-    filtered_emgB = fabs(filtered_emgB);
-
-    //moving average
-    B0=filtered_emgB;
-    MOVAVG_B=B0*0.1+B1*0.1+B2*0.1+B3*0.1+B4*0.1+B5*0.1+B6*0.1+B7*0.1+B8*0.1+B9*0.1;
-    B9=B8;
-    B8=B7;
-    B7=B6;
-    B6=B5;
-    B5=B4;
-    B4=B3;
-    B3=B2;
-    B2=B1;
-    B1=B0;
-
-    //naar scherm
-    scope.set(0,emg_valueB);        //ruwe data
-    scope.set(1,filtered_emgB);     //filtered
-    
-
-    scope.set(2,emg_valueT);        //ruwe data
-    scope.set(3,filtered_emgT);     //filtered
-    scope.send();
-}
-
 // *** MAIN ***
 int main()
-{ 
+{
+    pc.baud(115200);
     //bepaling van positie met triceps
     Ticker log_timerT;
-    arm_biquad_cascade_df1_init_f32(&lowpass,1,lowpass_const,lowpass_states);
-    arm_biquad_cascade_df1_init_f32(&highpass,1,highpass_const,highpass_states);
+    arm_biquad_cascade_df1_init_f32(&lowpassT,1,lowpass_const,lowpass_states);
+    arm_biquad_cascade_df1_init_f32(&highpassT,1,highpass_const,highpass_states);
 
     log_timerT.attach(Triceps, 0.005);
-    while(1) {}
+    wait(10);
+    log_timerT.detach();
+
+    wait(5);
 
+    Ticker log_timerB;
+    arm_biquad_cascade_df1_init_f32(&lowpassB,1,lowpass_const,lowpass_states);
+    arm_biquad_cascade_df1_init_f32(&highpassB,1,highpass_const,highpass_states);
+    
+    log_timerB.attach(Biceps,0.005);
+    wait(10);
+    log_timerB.detach();
+
+
+// positie van batje met behulp van Triceps
     drempelwaardeT1=4;
     drempelwaardeT2=7;
     if (MOVAVG_T >= drempelwaardeT1) {
@@ -207,9 +197,9 @@
         }
     }
     //bepaling van snelheidsstand met biceps
-    
-    arm_biquad_cascade_df1_init_f32(&lowpass,1,lowpass_const,lowpass_states);
-    arm_biquad_cascade_df1_init_f32(&highpass,1,highpass_const,highpass_states);
+
+    arm_biquad_cascade_df1_init_f32(&lowpassB,1,lowpass_const,lowpass_states);
+    arm_biquad_cascade_df1_init_f32(&highpassB,1,highpass_const,highpass_states);
 
 
 
@@ -251,4 +241,5 @@
             }
         }
     }
+
 }