Diff: main.cpp

Revision:

5:49c5553b6e2c

Parent:

4:d8fd3c4484cc

Child:

6:78494092a326

diff -r d8fd3c4484cc -r 49c5553b6e2c main.cpp
--- a/main.cpp	Wed Sep 30 22:50:24 2015 +0000
+++ b/main.cpp	Sun Oct 11 03:42:30 2015 +0000
@@ -1,9 +1,14 @@
 #include "mbed.h"
 #include "arm_math.h" 
 
-#define FLEXION   0.2f
-#define EXTENSION 0.2f
-#define FLEXION2  0.00f
+#define TRUE        1
+#define FALSE       0
+#define RAW         0
+#define RECTIFIED   1
+#define SMOOTH      2        
+#define THRESHOLDF  0.25f
+#define THRESHOLDE  0.25f
+#define FLEXION2    0.0f
 
 Ticker EMG_Sampler;
 Serial pc(SERIAL_TX, SERIAL_RX);
@@ -18,6 +23,7 @@
 PwmOut Pinky(D6);
 PwmOut ThumbRot(D5);
  
+//EMG samples and DSP variables
 float32_t EMG1, EMG2, EMG3;
 float32_t samples[25];
 float32_t samples2[25];
@@ -25,57 +31,60 @@
 float32_t abs_output[25];
 float32_t abs_output2[25];
 float32_t abs_output3[25];
-float32_t mean = 0.0f, mean2 = 0.0f, mean3 = 0.0f;
+float32_t mean = 0.0f;
+float32_t mean2 = 0.0f; 
+float32_t mean3 = 0.0f;
+
+//variables for state machines
 uint8_t state = 0;
+uint8_t action = 0;
+
+//conversion and data collection complete flag
 uint8_t COCO = 0;
 
+//functions declaration
+void PWM_Init(void);
 void ADC_Sampler(void);
-void Close(void);
-void Open(void);
-
+void RX_Interrupt(void);
+void Serial_Oscilloscope(uint8_t activation, uint8_t type);
+void FingerPosition(float32_t thumb_us, float32_t index_us, float32_t middle_us, float32_t pinky_us, float32_t thumbrot_us);
  
 int main() {
   pc.baud(115200);                                  //Serial com at 115200 bauds
+  pc.attach(&RX_Interrupt);                         //Serial RX interrupt attachment
   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);
-  //Open -> 2.4ms - Close 0.6ms
-  Index.period(0.02f*2.0f);
-  Index.pulsewidth(0.0024f*2.0f);
-  //Open -> 2.4ms - Close 0.6ms
-  Middle.period(0.02f*2.0f);
-  Middle.pulsewidth(0.0024f*2.0f);
-  //Open -> 0.6ms - Close 2.4ms
-  Pinky.period(0.02f*2.0f);
-  Pinky.pulsewidth(0.0006f*2.0f);
-  //Open -> 0.6ms - Close 2.4ms
-  //ThumbRot.period(0.02f/2.0f);
-  ThumbRot.pulsewidth(0.0006f*2.0f);
+  PWM_Init();                                       //Servo initialization
+  myled = 1;
   
-  myled = 1;
   while(1) { 
     if(COCO){
-        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);        //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);
+        arm_abs_f32(samples, abs_output, 25);       //rectifier EMG1
+        arm_abs_f32(samples2, abs_output2, 25);     //rectifier EMG2
+        arm_abs_f32(samples3, abs_output3, 25);     //rectifier EMG3
+        arm_mean_f32(abs_output, 25, &mean);        //mean EMG1
+        arm_mean_f32(abs_output2, 25, &mean2);      //mean EMG2 
+        arm_mean_f32(abs_output3, 25, &mean3);      //mean EMG3
+        Serial_Oscilloscope(FALSE,SMOOTH);
         switch(state){   
             case 0: {
-                if (mean>FLEXION){
+                if (mean>THRESHOLDF){
                     myled = 0;
                     state = 1;
-                    Close();
+                    switch(action){
+                        case '1': FingerPosition(2400, 600, 600,2400,2400); break;  //Close
+                        case '2': FingerPosition(2400,2400, 600,2400,2400); break;  //Point
+                        case '3': FingerPosition(2400, 600,2400, 600,2400); break;  //Pinch
+                        case '4': FingerPosition(2400, 600, 600,2400, 600); break;  //Lateral
+                        case '5': FingerPosition(2400, 600, 600, 600,2400); break;  //Tripod
+                        default:  FingerPosition(2400, 600, 600,2400,2400);         //Close
+                    }
                 }    
             } break;
             case 1: {
-                if (mean2>EXTENSION){
+                if (mean2>THRESHOLDE){
                     myled = 1;
                     state = 0;
-                    Open();
+                    FingerPosition(1000,2400,2400, 600, 600);                       //Open
                 }       
             }
         }
@@ -84,12 +93,12 @@
   }
 }
 
+//EMG sampler Ts = 1ms
 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);
+    Serial_Oscilloscope(TRUE,RAW);
     uint32_t m = __get_PRIMASK();
     __disable_irq();        
     for(int j=24;j>0;j--) {
@@ -104,18 +113,54 @@
     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);
+//action selection trough serial console
+void RX_Interrupt(void){
+    action = pc.getc();
+    /*switch(action){
+        case '1': pc.printf("Power Grip\n"); break;
+        case '2': pc.printf("Point\n"); break;
+        case '3': pc.printf("Pinch\n"); break;
+        case '4': pc.printf("Lateral\n"); break;
+        case '5': pc.printf("Tripod\n"); break;
+        default:  pc.printf("Power Grip\n");
+    }*/
 }
 
-void Open(void){
+//PWM initialization for servos
+void PWM_Init(void){
+    //Open -> 0.6ms - Close 2.4ms
+    Thumb.period(0.02f/2.0f);
     Thumb.pulsewidth(0.0010f/2.0f);
+    //Open -> 2.4ms - Close 0.6ms
+    Index.period(0.02f*2.0f);
     Index.pulsewidth(0.0024f*2.0f);
+    //Open -> 2.4ms - Close 0.6ms
+    Middle.period(0.02f*2.0f);
     Middle.pulsewidth(0.0024f*2.0f);
+    //Open -> 0.6ms - Close 2.4ms
+    Pinky.period(0.02f*2.0f);
     Pinky.pulsewidth(0.0006f*2.0f);
-    ThumbRot.pulsewidth(0.0006f*2.0f);
-}
\ No newline at end of file
+    //Open -> 0.6ms - Close 2.4ms
+    ThumbRot.pulsewidth(0.0006f*2.0f);  
+}
+
+//send data through serail port for oscilloscope visualization
+void Serial_Oscilloscope(uint8_t activation, uint8_t type){
+    if (activation){
+        switch(type){
+            case RAW: pc.printf("%.10f,%.10f,%.10f\n\r",EMG1,EMG2,EMG3); break; 
+            case RECTIFIED: pc.printf("%.10f,%.10f,%.10f\n\r",abs_output[0],abs_output2[0],abs_output3[0]);break;
+            case SMOOTH: pc.printf("%.10f,%.10f\n\r",mean,mean2,mean3); break;
+            default: pc.printf("%.10f,%.10f,%.10f\n\r",EMG1,EMG2,EMG3); 
+        } 
+    }    
+}
+
+//finger position through servos - values on microseconds (600 us - 2400 us)
+void FingerPosition(float32_t thumb_us, float32_t index_us, float32_t middle_us, float32_t pinky_us, float32_t thumbrot_us){
+    Thumb.pulsewidth(thumb_us/1000000.0f/2.0f);
+    Index.pulsewidth(index_us/1000000.0f*2.0f);
+    Middle.pulsewidth(middle_us/1000000.0f*2.0f);
+    Pinky.pulsewidth(pinky_us/1000000.0f*2.0f);
+    ThumbRot.pulsewidth(thumbrot_us/1000000.0f*2.0f);
+}