Galileo Hand Basic Example implemented on FRDM K64F
Dependencies: NOKIA_5110 mbed-dsp mbed
Fork of Nucleo_EMG_Galileo_Hand by
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?
User | Revision | Line number | New 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 | } |