Galileo Hand Basic Example implemented on FRDM K64F

Fork of Nucleo_EMG_Galileo_Hand by

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;

-    pc.printf("%.10f,%.10f,%.10f\n\r",EMG1,EMG2,EMG3);
-    __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];
-    }
-    COCO = 1;
-}
+void Close(void);
+void Open(void);
+

int main() {
pc.baud(115200);                                  //Serial com at 115200 bauds
//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;
}
}
}
+
+    //pc.printf("%.10f,%.10f,%.10f\n\r",EMG1,EMG2,EMG3);
+    //pc.printf("%.10f,%.10f\n\r",EMG1,EMG2);
+    __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;
+    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
```