Diff: main.cpp

Revision:

4:d8fd3c4484cc

Parent:

3:f784301a5166

Child:

5:49c5553b6e2c

--- a/main.cpp	Wed Sep 02 01:29:02 2015 +0000
+++ b/main.cpp	Wed Sep 30 22:50:24 2015 +0000
@@ -1,6 +1,10 @@
 #include "mbed.h"
 #include "arm_math.h" 
 
+#define FLEXION   0.2f
+#define EXTENSION 0.2f
+#define FLEXION2  0.00f
+
 Ticker EMG_Sampler;
 Serial pc(SERIAL_TX, SERIAL_RX);
 DigitalOut myled(LED1);
@@ -22,36 +26,17 @@
 float32_t abs_output2[25];
 float32_t abs_output3[25];
 float32_t mean = 0.0f, mean2 = 0.0f, mean3 = 0.0f;
+uint8_t state = 0;
 uint8_t COCO = 0;
 
-void ADC_Sampler() {
-    EMG1 = (E1.read()-Ref.read())*3.3f;
-    EMG2 = (E2.read()-Ref.read())*3.3f;
-    EMG3 = (E3.read()-Ref.read())*3.3f;
-    pc.printf("%.10f,%.10f,%.10f\n\r",EMG1,EMG2,EMG3);
-    uint32_t m = __get_PRIMASK();
-    __disable_irq();        
-    for(int j=99;j>0;j--) {
-        samples[j]=samples[j-1];                    //Fill Array
-        samples2[j]=samples2[j-1];                  //Fill Array
-        samples3[j]=samples3[j-1];                  //Fill Array
-    }
-    samples[0]=EMG1;
-    samples2[0]=EMG2;
-    samples3[0]=EMG3;
-    //copy 25 samples on array
-    for(int j=99;j>0;j--){ 
-        samples[j]=samples[j-1];
-        samples2[j]=samples2[j-1];
-        samples3[j]=samples3[j-1];
-    }       
-    __set_PRIMASK(m);
-    COCO = 1;
-}
+void ADC_Sampler(void);
+void Close(void);
+void Open(void);
+
  
 int main() {
   pc.baud(115200);                                  //Serial com at 115200 bauds
-  EMG_Sampler.attach(&ADC_Sampler, 0.001);          //1ms ticker for ADC Sampler
+  EMG_Sampler.attach(&ADC_Sampler, 0.001);          //1 ms ticker for ADC Sampler
   //Open -> 0.6ms - Close 2.4ms
   Thumb.period(0.02f/2.0f);
   Thumb.pulsewidth(0.0010f/2.0f);
@@ -71,30 +56,66 @@
   myled = 1;
   while(1) { 
     if(COCO){
-        //mean for threshold
         arm_abs_f32(samples, abs_output, 25);       //rectifier
         arm_abs_f32(samples2, abs_output2, 25);     //rectifier
         arm_abs_f32(samples3, abs_output3, 25);     //rectifier
-        arm_mean_f32(abs_output, 25, &mean);
-        arm_mean_f32(abs_output, 25, &mean2);
-        arm_mean_f32(abs_output, 25, &mean3); 
-        if ((mean>0.009f)&&(mean2>0.007f)&&(mean3>0.007f)) {
-            myled = 0;
-            Thumb.pulsewidth(0.0024f/2.0f);
-            Index.pulsewidth(0.0006f*2.0f);
-            Middle.pulsewidth(0.0006f*2.0f);
-            Pinky.pulsewidth(0.0024f*2.0f);
-            ThumbRot.pulsewidth(0.0024f*2.0f);
-        }
-        else {
-            myled = 1;
-            Thumb.pulsewidth(0.0010f/2.0f);
-            Index.pulsewidth(0.0024f*2.0f);
-            Middle.pulsewidth(0.0024f*2.0f);
-            Pinky.pulsewidth(0.0006f*2.0f);
-            ThumbRot.pulsewidth(0.0006f*2.0f);
+        arm_mean_f32(abs_output, 25, &mean);        //mean
+        arm_mean_f32(abs_output2, 25, &mean2);       //mean
+        arm_mean_f32(abs_output3, 25, &mean3);       //mean
+        //pc.printf("%.10f,%.10f\n\r",mean,mean2);
+        switch(state){   
+            case 0: {
+                if (mean>FLEXION){
+                    myled = 0;
+                    state = 1;
+                    Close();
+                }    
+            } break;
+            case 1: {
+                if (mean2>EXTENSION){
+                    myled = 1;
+                    state = 0;
+                    Open();
+                }       
+            }
         }
         COCO = 0;   
     }   
   }
 }
+
+void ADC_Sampler() {
+    EMG1 = (E1.read()-Ref.read())*3.3f;
+    EMG2 = (E2.read()-Ref.read())*3.3f;
+    EMG3 = (E3.read()-Ref.read())*3.3f;
+    //pc.printf("%.10f,%.10f,%.10f\n\r",EMG1,EMG2,EMG3);
+    //pc.printf("%.10f,%.10f\n\r",EMG1,EMG2);
+    uint32_t m = __get_PRIMASK();
+    __disable_irq();        
+    for(int j=24;j>0;j--) {
+        samples[j]=samples[j-1];                    //Fill Array
+        samples2[j]=samples2[j-1];                  //Fill Array
+        samples3[j]=samples3[j-1];                  //Fill Array
+    }
+    samples[0]=EMG1;
+    samples2[0]=EMG2;
+    samples3[0]=EMG3;       
+    __set_PRIMASK(m);
+    COCO = 1;
+}
+
+void Close(void){
+    Thumb.pulsewidth(0.0024f/2.0f);
+    Index.pulsewidth(0.0006f*2.0f);
+    Middle.pulsewidth(0.0006f*2.0f);
+    Pinky.pulsewidth(0.0024f*2.0f);
+    ThumbRot.pulsewidth(0.0024f*2.0f);
+}
+
+void Open(void){
+    Thumb.pulsewidth(0.0010f/2.0f);
+    Index.pulsewidth(0.0024f*2.0f);
+    Middle.pulsewidth(0.0024f*2.0f);
+    Pinky.pulsewidth(0.0006f*2.0f);
+    ThumbRot.pulsewidth(0.0006f*2.0f);
+}
\ No newline at end of file