Revision 2:a86b09b00008, committed 2014-10-21

Comitter:

s1340735

Date:

Tue Oct 21 16:14:14 2014 +0000

Parent:

1:6a8b45298e54

Child:

3:69ffa34e4239

Commit message:

bicep en tricep klaar. nu calibratie

Changed in this revision

--- a/main.cpp	Tue Oct 21 14:27:50 2014 +0000
+++ b/main.cpp	Tue Oct 21 16:14:14 2014 +0000
@@ -3,16 +3,27 @@
 #include "MODSERIAL.h"
 #include "arm_math.h"
 
+MODSERIAL pc(USBTX,USBRX);
+
 HIDScope scope(2);//is dit 4 voor 2 spieren? en hoe zit het met scope.set?
 
-AnalogIn emgB(PTB0);
+AnalogIn emgB(PTB0);//biceps
+AnalogIn emgT(PTB1); // tricep
 
 //*** OBJECTS ***
-float (filtered_emgB);
+//bicep
+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
+float filtered_emgT;
+float drempelwaardeT1, drempelwaardeT2;//T1=positie 1, T2=positie 2
+int yT1, yT2;
+float T0, T1, T2, T3, T4, T5, T6, T7, T8, T9, MOVAVG_T;//moving average objects
+int positie;
+
 
 //*** FILTERS ***
 arm_biquad_casd_df1_inst_f32 lowpass;
@@ -25,9 +36,40 @@
 float highpass_const[] = {0.8005910267, -1.6011820533, 0.8005910267, 1.5610153913, -0.6413487154};//{a0 a1 a2 -b1 -b2}
 float highpass_states[4];
 
-//*** CALIBRATIE ***
+//*** CALIBRATIE ***//dit moet nog in de main komen! en ik snap dit niet :(
+//void Calibratie()
+//{
+//pc.printf("Calibratie drempelwaarde Triceps stand 1\n");
+//wait(0.5);
+//   {
+//  int i;
+// int j=19;
+
+//for (i=0, i<=j; i++) {
+/*variable to store value in*/
+//  uint16_t emg_valueT1i_C;
 
+//  float emg_value_f32T1i_C;
+/*put raw emg value both in red and in emg_value*/
+//  emg_valueT1i_C = emgT1i_C.read_u16(); // read direct ADC result, converted to 16 bit integer (0..2^16 = 0..65536 = 0..3.3V)
+//  emg_value_f32T1i_C = emgT1i_C.read();
+
+//process emg
+//  arm_biquad_cascade_df1_f32(&highpass, &emg_value_f32T1i_C, &filtered_emgT1i_C, 1 );
+// filtered_emgT1i_C = fabs(filtered_emgT1i_C);
+// arm_biquad_cascade_df1_f32(&lowpass, &filtered_emgT1i_C, &filtered_emgT1i_C, 1 );
+//    }
+// }
+//}
+//******************************
+//Mean Triceps stand 1
+//void MeanTriceps()
+//{
 //
+//    float MeanT1=filtered_emgT10_C*0.05+filtered_emgT11_C*0.05+filtered_emgT12_C*0.05+filtered_emgT13_C*0.05+filtered_emgT14_C*0.05+filtered_emgT15_C*0.05+filtered_emgT16_C*0.05+filtered_emgT17_C*0.05+filtered_emgT18_C*0.05+filtered_emgT19_C*0.05+filtered_emgT110_C*0.05+filtered_emgT111_C*0.05+filtered_emgT112_C*0.05+filtered_emgT113_C*0.05+filtered_emgT114_C*0.05+filtered_emgT115_C*0.05+filtered_emgT116_C*0.05+filtered_emgT117_C*0.05+filtered_emgT118_C*0.05+filtered_emgT119_C*0.05;
+//
+//}
+//******************************
 
 //*** BICEP EMG ***
 void Biceps()
@@ -35,25 +77,18 @@
     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;
@@ -64,59 +99,130 @@
     B2=B1;
     B1=B0;
 
-    //naar scherm sturen
-    scope.set(0,emg_valueB);
-    scope.set(1,MOVAVG_B);
+    //naar scherm
+    scope.set(0,emg_valueB);        //ruwe data
+    scope.set(1,filtered_emgB);     //filtered
     scope.send();
+}
 
 // *** TRICEP EMG ***
-void Triceps(){
+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;
+
+    //naar scherm
+    scope.set(2,emg_valueT);        //ruwe data
+    scope.set(3,filtered_emgT);     //filtered
+    scope.send();
+}
 
 // *** MAIN ***
-    int main() {
+int main()
+{
+    //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);
+
+    log_timerT.attach(Triceps, 0.005);
+    while(1) {}
 
-        Ticker log_timer;
-        arm_biquad_cascade_df1_init_f32(&lowpass,1,lowpass_const,lowpass_states);
-        arm_biquad_cascade_df1_init_f32(&highpass,1,highpass_const,highpass_states);
+    drempelwaardeT1=4.99;
+    drempelwaardeT2=7;
+    if (MOVAVG_T >= drempelwaardeT1) {
+        yT1=1;
+        if (MOVAVG_T >= drempelwaardeT1) {
+            yT2=1;
+        } else {
+            yT2=0;
+        }
+    } else {
+        yT1=0;
+    }
 
-        log_timer.attach(Biceps, 0.005);
-        while(1) {}
+    positie=yT1+yT2;//INPUT MOTOR 2
 
-        //aansturing
-        if (filtered_emgB >= drempelwaardeB1) {
-            yB1=1;
-            if (filtered_emgB >= drempelwaardeB2) {
-                yB2=1;
-                if (filtered_emgB >= drempelwaardeB3) {
-                    yB3=1;
-                } else {
-                    yB3=0;
-                }
+    //controle positie op scherm
+    if (positie==0) {
+        pc.printf("Motor 2 gaat naar stand 0\n");
+    } else {
+        if (positie==1) {
+            pc.printf("Motor 2 gaat naar stand 1\n");
+        } else {
+            if (positie==2) {
+                pc.printf("Motor 1 beweegt met snelheid 2\n");
+            }
+        }
+    }
+
+
+    //bepaling van snelheidsstand met biceps
+    Ticker log_timerB;
+    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_timerB.attach(Biceps, 0.005);
+    while(1) {}
+
+    drempelwaardeB1=4.99;
+    drempelwaardeB2=6;
+    drempelwaardeB3=10;
+
+    if (MOVAVG_B >= drempelwaardeB1) {
+        yB1=1;
+        if (MOVAVG_B >= drempelwaardeB2) {
+            yB2=1;
+            if (MOVAVG_B >= drempelwaardeB3) {
+                yB3=1;
             } else {
-                yB2=0;
+                yB3=0;
             }
         } else {
-            yB1=0;
+            yB2=0;
         }
+    } else {
+        yB1=0;
+    }
 
-        //controle
-        snelheidsstand=yB1+yB2+yB3;
+    snelheidsstand=yB1+yB2+yB3;//INPUT MOTOR 1
 
+    //controle snelheidsstand op scherm
+    if (snelheidsstand==0) {
+        pc.printf("Motor 1 beweegt niet\n");
+    } else {
         if (snelheidsstand==1) {
             pc.printf("Motor 1 beweegt met snelheid 1\n");
         } else {
-            pc.printf("Motor 1 beweegt niet met snelheid 1\n");
-        }
-        if (snelheidsstand==2) {
-            pc.printf("Motor 1 beweegt met snelheid 2\n");
-        } else {
-            pc.printf("Motor 1 beweegt niet met snelheid 2\n");
-        }
-        if (snelheidsstand==3) {
-            pc.printf("Motor 1 beweegt met snelheid 3\n");
-        } else {
-            pc.printf("Motor 1 beweegt niet met snelheid 3\n");
+            if (snelheidsstand==2) {
+                pc.printf("Motor 1 beweegt met snelheid 2\n");
+            } else {
+                if (snelheidsstand==3) {
+                    pc.printf("Motor 1 beweegt met snelheid 3\n");
+                }
+            }
         }
     }
 }
-