Julio Fajardo / Mbed 2 deprecated FRDM_EMG_Galileo_Hand_Basic_Example

Galileo Hand Basic Example implemented on FRDM K64F

Dependencies:   NOKIA_5110 mbed-dsp mbed

Fork of Nucleo_EMG_Galileo_Hand by Julio Fajardo

main.cpp@4:d8fd3c4484cc, 2015-09-30 (annotated)

Committer:
julioefajardo
Date:
Wed Sep 30 22:50:24 2015 +0000
Revision:
4:d8fd3c4484cc
Parent:
3:f784301a5166
Child:
5:49c5553b6e2c
Version 1.2

Who changed what in which revision?

UserRevisionLine numberNew contents of line
julioefajardo 0:f2b89c6a8a16 1 #include "mbed.h"
julioefajardo 0:f2b89c6a8a16 2 #include "arm_math.h"
julioefajardo 0:f2b89c6a8a16 3
julioefajardo 4:d8fd3c4484cc 4 #define FLEXION 0.2f
julioefajardo 4:d8fd3c4484cc 5 #define EXTENSION 0.2f
julioefajardo 4:d8fd3c4484cc 6 #define FLEXION2 0.00f
julioefajardo 4:d8fd3c4484cc 7
julioefajardo 0:f2b89c6a8a16 8 Ticker EMG_Sampler;
julioefajardo 0:f2b89c6a8a16 9 Serial pc(SERIAL_TX, SERIAL_RX);
julioefajardo 0:f2b89c6a8a16 10 DigitalOut myled(LED1);
julioefajardo 0:f2b89c6a8a16 11 AnalogIn Ref(A0);
julioefajardo 0:f2b89c6a8a16 12 AnalogIn E1(A1);
julioefajardo 0:f2b89c6a8a16 13 AnalogIn E2(A2);
julioefajardo 0:f2b89c6a8a16 14 AnalogIn E3(A3);
julioefajardo 3:f784301a5166 15 PwmOut Thumb(D11);
julioefajardo 3:f784301a5166 16 PwmOut Index(D10);
julioefajardo 3:f784301a5166 17 PwmOut Middle(D9);
julioefajardo 3:f784301a5166 18 PwmOut Pinky(D6);
julioefajardo 3:f784301a5166 19 PwmOut ThumbRot(D5);
julioefajardo 0:f2b89c6a8a16 20
julioefajardo 0:f2b89c6a8a16 21 float32_t EMG1, EMG2, EMG3;
julioefajardo 2:12f979d695db 22 float32_t samples[25];
julioefajardo 2:12f979d695db 23 float32_t samples2[25];
julioefajardo 2:12f979d695db 24 float32_t samples3[25];
julioefajardo 2:12f979d695db 25 float32_t abs_output[25];
julioefajardo 2:12f979d695db 26 float32_t abs_output2[25];
julioefajardo 2:12f979d695db 27 float32_t abs_output3[25];
julioefajardo 0:f2b89c6a8a16 28 float32_t mean = 0.0f, mean2 = 0.0f, mean3 = 0.0f;
julioefajardo 4:d8fd3c4484cc 29 uint8_t state = 0;
julioefajardo 0:f2b89c6a8a16 30 uint8_t COCO = 0;
julioefajardo 0:f2b89c6a8a16 31
julioefajardo 4:d8fd3c4484cc 32 void ADC_Sampler(void);
julioefajardo 4:d8fd3c4484cc 33 void Close(void);
julioefajardo 4:d8fd3c4484cc 34 void Open(void);
julioefajardo 4:d8fd3c4484cc 35
julioefajardo 0:f2b89c6a8a16 36
julioefajardo 0:f2b89c6a8a16 37 int main() {
julioefajardo 0:f2b89c6a8a16 38 pc.baud(115200); //Serial com at 115200 bauds
julioefajardo 4:d8fd3c4484cc 39 EMG_Sampler.attach(&ADC_Sampler, 0.001); //1 ms ticker for ADC Sampler
julioefajardo 3:f784301a5166 40 //Open -> 0.6ms - Close 2.4ms
julioefajardo 3:f784301a5166 41 Thumb.period(0.02f/2.0f);
julioefajardo 3:f784301a5166 42 Thumb.pulsewidth(0.0010f/2.0f);
julioefajardo 3:f784301a5166 43 //Open -> 2.4ms - Close 0.6ms
julioefajardo 3:f784301a5166 44 Index.period(0.02f*2.0f);
julioefajardo 3:f784301a5166 45 Index.pulsewidth(0.0024f*2.0f);
julioefajardo 3:f784301a5166 46 //Open -> 2.4ms - Close 0.6ms
julioefajardo 3:f784301a5166 47 Middle.period(0.02f*2.0f);
julioefajardo 3:f784301a5166 48 Middle.pulsewidth(0.0024f*2.0f);
julioefajardo 3:f784301a5166 49 //Open -> 0.6ms - Close 2.4ms
julioefajardo 3:f784301a5166 50 Pinky.period(0.02f*2.0f);
julioefajardo 3:f784301a5166 51 Pinky.pulsewidth(0.0006f*2.0f);
julioefajardo 3:f784301a5166 52 //Open -> 0.6ms - Close 2.4ms
julioefajardo 3:f784301a5166 53 //ThumbRot.period(0.02f/2.0f);
julioefajardo 3:f784301a5166 54 ThumbRot.pulsewidth(0.0006f*2.0f);
julioefajardo 3:f784301a5166 55
julioefajardo 0:f2b89c6a8a16 56 myled = 1;
julioefajardo 0:f2b89c6a8a16 57 while(1) {
julioefajardo 0:f2b89c6a8a16 58 if(COCO){
julioefajardo 2:12f979d695db 59 arm_abs_f32(samples, abs_output, 25); //rectifier
julioefajardo 2:12f979d695db 60 arm_abs_f32(samples2, abs_output2, 25); //rectifier
julioefajardo 2:12f979d695db 61 arm_abs_f32(samples3, abs_output3, 25); //rectifier
julioefajardo 4:d8fd3c4484cc 62 arm_mean_f32(abs_output, 25, &mean); //mean
julioefajardo 4:d8fd3c4484cc 63 arm_mean_f32(abs_output2, 25, &mean2); //mean
julioefajardo 4:d8fd3c4484cc 64 arm_mean_f32(abs_output3, 25, &mean3); //mean
julioefajardo 4:d8fd3c4484cc 65 //pc.printf("%.10f,%.10f\n\r",mean,mean2);
julioefajardo 4:d8fd3c4484cc 66 switch(state){
julioefajardo 4:d8fd3c4484cc 67 case 0: {
julioefajardo 4:d8fd3c4484cc 68 if (mean>FLEXION){
julioefajardo 4:d8fd3c4484cc 69 myled = 0;
julioefajardo 4:d8fd3c4484cc 70 state = 1;
julioefajardo 4:d8fd3c4484cc 71 Close();
julioefajardo 4:d8fd3c4484cc 72 }
julioefajardo 4:d8fd3c4484cc 73 } break;
julioefajardo 4:d8fd3c4484cc 74 case 1: {
julioefajardo 4:d8fd3c4484cc 75 if (mean2>EXTENSION){
julioefajardo 4:d8fd3c4484cc 76 myled = 1;
julioefajardo 4:d8fd3c4484cc 77 state = 0;
julioefajardo 4:d8fd3c4484cc 78 Open();
julioefajardo 4:d8fd3c4484cc 79 }
julioefajardo 4:d8fd3c4484cc 80 }
julioefajardo 0:f2b89c6a8a16 81 }
julioefajardo 0:f2b89c6a8a16 82 COCO = 0;
julioefajardo 0:f2b89c6a8a16 83 }
julioefajardo 0:f2b89c6a8a16 84 }
julioefajardo 0:f2b89c6a8a16 85 }
julioefajardo 4:d8fd3c4484cc 86
julioefajardo 4:d8fd3c4484cc 87 void ADC_Sampler() {
julioefajardo 4:d8fd3c4484cc 88 EMG1 = (E1.read()-Ref.read())*3.3f;
julioefajardo 4:d8fd3c4484cc 89 EMG2 = (E2.read()-Ref.read())*3.3f;
julioefajardo 4:d8fd3c4484cc 90 EMG3 = (E3.read()-Ref.read())*3.3f;
julioefajardo 4:d8fd3c4484cc 91 //pc.printf("%.10f,%.10f,%.10f\n\r",EMG1,EMG2,EMG3);
julioefajardo 4:d8fd3c4484cc 92 //pc.printf("%.10f,%.10f\n\r",EMG1,EMG2);
julioefajardo 4:d8fd3c4484cc 93 uint32_t m = __get_PRIMASK();
julioefajardo 4:d8fd3c4484cc 94 __disable_irq();
julioefajardo 4:d8fd3c4484cc 95 for(int j=24;j>0;j--) {
julioefajardo 4:d8fd3c4484cc 96 samples[j]=samples[j-1]; //Fill Array
julioefajardo 4:d8fd3c4484cc 97 samples2[j]=samples2[j-1]; //Fill Array
julioefajardo 4:d8fd3c4484cc 98 samples3[j]=samples3[j-1]; //Fill Array
julioefajardo 4:d8fd3c4484cc 99 }
julioefajardo 4:d8fd3c4484cc 100 samples[0]=EMG1;
julioefajardo 4:d8fd3c4484cc 101 samples2[0]=EMG2;
julioefajardo 4:d8fd3c4484cc 102 samples3[0]=EMG3;
julioefajardo 4:d8fd3c4484cc 103 __set_PRIMASK(m);
julioefajardo 4:d8fd3c4484cc 104 COCO = 1;
julioefajardo 4:d8fd3c4484cc 105 }
julioefajardo 4:d8fd3c4484cc 106
julioefajardo 4:d8fd3c4484cc 107 void Close(void){
julioefajardo 4:d8fd3c4484cc 108 Thumb.pulsewidth(0.0024f/2.0f);
julioefajardo 4:d8fd3c4484cc 109 Index.pulsewidth(0.0006f*2.0f);
julioefajardo 4:d8fd3c4484cc 110 Middle.pulsewidth(0.0006f*2.0f);
julioefajardo 4:d8fd3c4484cc 111 Pinky.pulsewidth(0.0024f*2.0f);
julioefajardo 4:d8fd3c4484cc 112 ThumbRot.pulsewidth(0.0024f*2.0f);
julioefajardo 4:d8fd3c4484cc 113 }
julioefajardo 4:d8fd3c4484cc 114
julioefajardo 4:d8fd3c4484cc 115 void Open(void){
julioefajardo 4:d8fd3c4484cc 116 Thumb.pulsewidth(0.0010f/2.0f);
julioefajardo 4:d8fd3c4484cc 117 Index.pulsewidth(0.0024f*2.0f);
julioefajardo 4:d8fd3c4484cc 118 Middle.pulsewidth(0.0024f*2.0f);
julioefajardo 4:d8fd3c4484cc 119 Pinky.pulsewidth(0.0006f*2.0f);
julioefajardo 4:d8fd3c4484cc 120 ThumbRot.pulsewidth(0.0006f*2.0f);
julioefajardo 4:d8fd3c4484cc 121 }