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@0:f2b89c6a8a16, 2015-08-12 (annotated)

Committer:
julioefajardo
Date:
Wed Aug 12 17:52:38 2015 +0000
Revision:
0:f2b89c6a8a16
Child:
1:9472990d8bb4
EMG Hand Prosthesis Controller v1

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 0:f2b89c6a8a16 4 Ticker EMG_Sampler;
julioefajardo 0:f2b89c6a8a16 5 Serial pc(SERIAL_TX, SERIAL_RX);
julioefajardo 0:f2b89c6a8a16 6 DigitalOut myled(LED1);
julioefajardo 0:f2b89c6a8a16 7 AnalogIn Ref(A0);
julioefajardo 0:f2b89c6a8a16 8 AnalogIn E1(A1);
julioefajardo 0:f2b89c6a8a16 9 AnalogIn E2(A2);
julioefajardo 0:f2b89c6a8a16 10 AnalogIn E3(A3);
julioefajardo 0:f2b89c6a8a16 11 Servo Thumb(D11);
julioefajardo 0:f2b89c6a8a16 12
julioefajardo 0:f2b89c6a8a16 13 float32_t EMG1, EMG2, EMG3;
julioefajardo 0:f2b89c6a8a16 14 float32_t samples[100];
julioefajardo 0:f2b89c6a8a16 15 float32_t samples2[100];
julioefajardo 0:f2b89c6a8a16 16 float32_t samples3[100];
julioefajardo 0:f2b89c6a8a16 17 float32_t abs_output[100];
julioefajardo 0:f2b89c6a8a16 18 float32_t abs_output2[100];
julioefajardo 0:f2b89c6a8a16 19 float32_t abs_output3[100];
julioefajardo 0:f2b89c6a8a16 20 float32_t samplesfi[128];
julioefajardo 0:f2b89c6a8a16 21 float32_t samplesfi2[128];
julioefajardo 0:f2b89c6a8a16 22 float32_t samplesfi3[128];
julioefajardo 0:f2b89c6a8a16 23 float32_t mean = 0.0f, mean2 = 0.0f, mean3 = 0.0f;
julioefajardo 0:f2b89c6a8a16 24 uint8_t COCO = 0;
julioefajardo 0:f2b89c6a8a16 25
julioefajardo 0:f2b89c6a8a16 26 void ADC_Sampler() {
julioefajardo 0:f2b89c6a8a16 27 EMG1 = (E1.read()-Ref.read())*3.3f;
julioefajardo 0:f2b89c6a8a16 28 EMG2 = (E2.read()-Ref.read())*3.3f;
julioefajardo 0:f2b89c6a8a16 29 EMG3 = (E3.read()-Ref.read())*3.3f;
julioefajardo 0:f2b89c6a8a16 30 pc.printf("%.10f,%.10f,%.10f\n\r",EMG1,EMG2,EMG3);
julioefajardo 0:f2b89c6a8a16 31 uint32_t m = __get_PRIMASK();
julioefajardo 0:f2b89c6a8a16 32 __disable_irq();
julioefajardo 0:f2b89c6a8a16 33 for(int j=99;j>0;j--) {
julioefajardo 0:f2b89c6a8a16 34 samples[j]=samples[j-1]; //Fill Array
julioefajardo 0:f2b89c6a8a16 35 samples2[j]=samples2[j-1]; //Fill Array
julioefajardo 0:f2b89c6a8a16 36 samples3[j]=samples3[j-1]; //Fill Array
julioefajardo 0:f2b89c6a8a16 37 }
julioefajardo 0:f2b89c6a8a16 38 samples[0]=EMG1;
julioefajardo 0:f2b89c6a8a16 39 samples2[0]=EMG2;
julioefajardo 0:f2b89c6a8a16 40 samples3[0]=EMG3;
julioefajardo 0:f2b89c6a8a16 41 //copy 25 samples on array
julioefajardo 0:f2b89c6a8a16 42 for(int j=99;j>0;j--){
julioefajardo 0:f2b89c6a8a16 43 samples[j]=samples[j-1];
julioefajardo 0:f2b89c6a8a16 44 samples2[j]=samples2[j-1];
julioefajardo 0:f2b89c6a8a16 45 samples3[j]=samples3[j-1];
julioefajardo 0:f2b89c6a8a16 46 }
julioefajardo 0:f2b89c6a8a16 47 __set_PRIMASK(m);
julioefajardo 0:f2b89c6a8a16 48 COCO = 1;
julioefajardo 0:f2b89c6a8a16 49 }
julioefajardo 0:f2b89c6a8a16 50
julioefajardo 0:f2b89c6a8a16 51 float32_t h[29] = {
julioefajardo 0:f2b89c6a8a16 52 0.005122283331907f,0.005887141527430f,0.008108906860497f,0.011692906924201f,0.016472293441426f,0.022216298876727f,0.028641952887652f,0.035428649080362f,
julioefajardo 0:f2b89c6a8a16 53 0.042234794064957f,0.048715653723188f,0.054541442583407f,0.059414685262420f,0.063085915202651f,0.065366863558607f,0.0661404253491338f,0.065366863558607f,
julioefajardo 0:f2b89c6a8a16 54 0.063085915202651f,0.059414685262420f,0.054541442583407f,0.048715653723188f,0.042234794064957f,0.035428649080362f,0.028641952887652f,0.022216298876727f,
julioefajardo 0:f2b89c6a8a16 55 0.016472293441426f,0.011692906924201f,0.008108906860497f,0.005887141527430f,0.005122283331907f
julioefajardo 0:f2b89c6a8a16 56 };
julioefajardo 0:f2b89c6a8a16 57
julioefajardo 0:f2b89c6a8a16 58 int main() {
julioefajardo 0:f2b89c6a8a16 59 pc.baud(115200); //Serial com at 115200 bauds
julioefajardo 0:f2b89c6a8a16 60 EMG_Sampler.attach(&ADC_Sampler, 0.001); //1ms ticker for ADC Sampler
julioefajardo 0:f2b89c6a8a16 61 myled = 1;
julioefajardo 0:f2b89c6a8a16 62 //Thumb = 0;
julioefajardo 0:f2b89c6a8a16 63 while(1) {
julioefajardo 0:f2b89c6a8a16 64 if(COCO){
julioefajardo 0:f2b89c6a8a16 65 //mean for threshold
julioefajardo 0:f2b89c6a8a16 66 arm_abs_f32(samples, abs_output, 100); //rectifier
julioefajardo 0:f2b89c6a8a16 67 arm_abs_f32(samples2, abs_output2, 100); //rectifier
julioefajardo 0:f2b89c6a8a16 68 arm_abs_f32(samples3, abs_output3, 100); //rectifier
julioefajardo 0:f2b89c6a8a16 69 arm_conv_f32(abs_output,100,h,29,samplesfi);//low pass filter
julioefajardo 0:f2b89c6a8a16 70 arm_conv_f32(abs_output2,100,h,29,samplesfi2);//low pass filter
julioefajardo 0:f2b89c6a8a16 71 arm_conv_f32(abs_output3,100,h,29,samplesfi3);//low pass filter
julioefajardo 0:f2b89c6a8a16 72 arm_mean_f32(samplesfi, 128, &mean);
julioefajardo 0:f2b89c6a8a16 73 arm_mean_f32(samplesfi2, 128, &mean2);
julioefajardo 0:f2b89c6a8a16 74 arm_mean_f32(samplesfi3, 128, &mean3);
julioefajardo 0:f2b89c6a8a16 75 if ((mean>0.009f)&&(mean2>0.007f)&&(mean3>0.007f)) {
julioefajardo 0:f2b89c6a8a16 76 myled = 0;
julioefajardo 0:f2b89c6a8a16 77 }
julioefajardo 0:f2b89c6a8a16 78 else {
julioefajardo 0:f2b89c6a8a16 79 myled = 1;
julioefajardo 0:f2b89c6a8a16 80 }
julioefajardo 0:f2b89c6a8a16 81 COCO = 0;
julioefajardo 0:f2b89c6a8a16 82 }
julioefajardo 0:f2b89c6a8a16 83 }
julioefajardo 0:f2b89c6a8a16 84 }