Galileo Hand Basic Example implemented on FRDM K64F
Dependencies: NOKIA_5110 mbed-dsp mbed
Fork of Nucleo_EMG_Galileo_Hand by
main.cpp@6:78494092a326, 2015-11-01 (annotated)
- Committer:
- julioefajardo
- Date:
- Sun Nov 01 18:35:45 2015 +0000
- Revision:
- 6:78494092a326
- Parent:
- 5:49c5553b6e2c
FRDM EMG Galileo Hand Basic Examples
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 | 5:49c5553b6e2c | 4 | #define TRUE 1 |
julioefajardo | 5:49c5553b6e2c | 5 | #define FALSE 0 |
julioefajardo | 5:49c5553b6e2c | 6 | #define RAW 0 |
julioefajardo | 5:49c5553b6e2c | 7 | #define RECTIFIED 1 |
julioefajardo | 5:49c5553b6e2c | 8 | #define SMOOTH 2 |
julioefajardo | 5:49c5553b6e2c | 9 | #define THRESHOLDF 0.25f |
julioefajardo | 5:49c5553b6e2c | 10 | #define THRESHOLDE 0.25f |
julioefajardo | 5:49c5553b6e2c | 11 | #define FLEXION2 0.0f |
julioefajardo | 4:d8fd3c4484cc | 12 | |
julioefajardo | 0:f2b89c6a8a16 | 13 | Ticker EMG_Sampler; |
julioefajardo | 6:78494092a326 | 14 | //Serial pc(SERIAL_TX, SERIAL_RX); |
julioefajardo | 6:78494092a326 | 15 | Serial pc(USBTX, USBRX); |
julioefajardo | 0:f2b89c6a8a16 | 16 | DigitalOut myled(LED1); |
julioefajardo | 6:78494092a326 | 17 | InterruptIn sw2(SW2); |
julioefajardo | 0:f2b89c6a8a16 | 18 | AnalogIn Ref(A0); |
julioefajardo | 0:f2b89c6a8a16 | 19 | AnalogIn E1(A1); |
julioefajardo | 0:f2b89c6a8a16 | 20 | AnalogIn E2(A2); |
julioefajardo | 0:f2b89c6a8a16 | 21 | AnalogIn E3(A3); |
julioefajardo | 3:f784301a5166 | 22 | PwmOut Thumb(D11); |
julioefajardo | 3:f784301a5166 | 23 | PwmOut Index(D10); |
julioefajardo | 3:f784301a5166 | 24 | PwmOut Middle(D9); |
julioefajardo | 3:f784301a5166 | 25 | PwmOut Pinky(D6); |
julioefajardo | 3:f784301a5166 | 26 | PwmOut ThumbRot(D5); |
julioefajardo | 0:f2b89c6a8a16 | 27 | |
julioefajardo | 5:49c5553b6e2c | 28 | //EMG samples and DSP variables |
julioefajardo | 0:f2b89c6a8a16 | 29 | float32_t EMG1, EMG2, EMG3; |
julioefajardo | 2:12f979d695db | 30 | float32_t samples[25]; |
julioefajardo | 2:12f979d695db | 31 | float32_t samples2[25]; |
julioefajardo | 2:12f979d695db | 32 | float32_t samples3[25]; |
julioefajardo | 2:12f979d695db | 33 | float32_t abs_output[25]; |
julioefajardo | 2:12f979d695db | 34 | float32_t abs_output2[25]; |
julioefajardo | 2:12f979d695db | 35 | float32_t abs_output3[25]; |
julioefajardo | 5:49c5553b6e2c | 36 | float32_t mean = 0.0f; |
julioefajardo | 5:49c5553b6e2c | 37 | float32_t mean2 = 0.0f; |
julioefajardo | 5:49c5553b6e2c | 38 | float32_t mean3 = 0.0f; |
julioefajardo | 5:49c5553b6e2c | 39 | |
julioefajardo | 5:49c5553b6e2c | 40 | //variables for state machines |
julioefajardo | 4:d8fd3c4484cc | 41 | uint8_t state = 0; |
julioefajardo | 6:78494092a326 | 42 | uint8_t action = '0'; |
julioefajardo | 5:49c5553b6e2c | 43 | |
julioefajardo | 5:49c5553b6e2c | 44 | //conversion and data collection complete flag |
julioefajardo | 0:f2b89c6a8a16 | 45 | uint8_t COCO = 0; |
julioefajardo | 0:f2b89c6a8a16 | 46 | |
julioefajardo | 5:49c5553b6e2c | 47 | //functions declaration |
julioefajardo | 5:49c5553b6e2c | 48 | void PWM_Init(void); |
julioefajardo | 4:d8fd3c4484cc | 49 | void ADC_Sampler(void); |
julioefajardo | 5:49c5553b6e2c | 50 | void RX_Interrupt(void); |
julioefajardo | 5:49c5553b6e2c | 51 | void Serial_Oscilloscope(uint8_t activation, uint8_t type); |
julioefajardo | 5:49c5553b6e2c | 52 | void FingerPosition(float32_t thumb_us, float32_t index_us, float32_t middle_us, float32_t pinky_us, float32_t thumbrot_us); |
julioefajardo | 6:78494092a326 | 53 | |
julioefajardo | 6:78494092a326 | 54 | void flip() { |
julioefajardo | 6:78494092a326 | 55 | if(action<='5') action++; |
julioefajardo | 6:78494092a326 | 56 | else action = '1'; |
julioefajardo | 6:78494092a326 | 57 | } |
julioefajardo | 6:78494092a326 | 58 | |
julioefajardo | 0:f2b89c6a8a16 | 59 | int main() { |
julioefajardo | 0:f2b89c6a8a16 | 60 | pc.baud(115200); //Serial com at 115200 bauds |
julioefajardo | 5:49c5553b6e2c | 61 | pc.attach(&RX_Interrupt); //Serial RX interrupt attachment |
julioefajardo | 6:78494092a326 | 62 | sw2.rise(&flip); |
julioefajardo | 4:d8fd3c4484cc | 63 | EMG_Sampler.attach(&ADC_Sampler, 0.001); //1 ms ticker for ADC Sampler |
julioefajardo | 5:49c5553b6e2c | 64 | PWM_Init(); //Servo initialization |
julioefajardo | 5:49c5553b6e2c | 65 | myled = 1; |
julioefajardo | 3:f784301a5166 | 66 | |
julioefajardo | 0:f2b89c6a8a16 | 67 | while(1) { |
julioefajardo | 0:f2b89c6a8a16 | 68 | if(COCO){ |
julioefajardo | 5:49c5553b6e2c | 69 | arm_abs_f32(samples, abs_output, 25); //rectifier EMG1 |
julioefajardo | 5:49c5553b6e2c | 70 | arm_abs_f32(samples2, abs_output2, 25); //rectifier EMG2 |
julioefajardo | 5:49c5553b6e2c | 71 | arm_abs_f32(samples3, abs_output3, 25); //rectifier EMG3 |
julioefajardo | 5:49c5553b6e2c | 72 | arm_mean_f32(abs_output, 25, &mean); //mean EMG1 |
julioefajardo | 5:49c5553b6e2c | 73 | arm_mean_f32(abs_output2, 25, &mean2); //mean EMG2 |
julioefajardo | 5:49c5553b6e2c | 74 | arm_mean_f32(abs_output3, 25, &mean3); //mean EMG3 |
julioefajardo | 6:78494092a326 | 75 | Serial_Oscilloscope(TRUE,SMOOTH); |
julioefajardo | 4:d8fd3c4484cc | 76 | switch(state){ |
julioefajardo | 4:d8fd3c4484cc | 77 | case 0: { |
julioefajardo | 5:49c5553b6e2c | 78 | if (mean>THRESHOLDF){ |
julioefajardo | 4:d8fd3c4484cc | 79 | myled = 0; |
julioefajardo | 4:d8fd3c4484cc | 80 | state = 1; |
julioefajardo | 5:49c5553b6e2c | 81 | switch(action){ |
julioefajardo | 5:49c5553b6e2c | 82 | case '1': FingerPosition(2400, 600, 600,2400,2400); break; //Close |
julioefajardo | 5:49c5553b6e2c | 83 | case '2': FingerPosition(2400,2400, 600,2400,2400); break; //Point |
julioefajardo | 5:49c5553b6e2c | 84 | case '3': FingerPosition(2400, 600,2400, 600,2400); break; //Pinch |
julioefajardo | 5:49c5553b6e2c | 85 | case '4': FingerPosition(2400, 600, 600,2400, 600); break; //Lateral |
julioefajardo | 5:49c5553b6e2c | 86 | case '5': FingerPosition(2400, 600, 600, 600,2400); break; //Tripod |
julioefajardo | 5:49c5553b6e2c | 87 | default: FingerPosition(2400, 600, 600,2400,2400); //Close |
julioefajardo | 5:49c5553b6e2c | 88 | } |
julioefajardo | 4:d8fd3c4484cc | 89 | } |
julioefajardo | 4:d8fd3c4484cc | 90 | } break; |
julioefajardo | 4:d8fd3c4484cc | 91 | case 1: { |
julioefajardo | 5:49c5553b6e2c | 92 | if (mean2>THRESHOLDE){ |
julioefajardo | 4:d8fd3c4484cc | 93 | myled = 1; |
julioefajardo | 4:d8fd3c4484cc | 94 | state = 0; |
julioefajardo | 6:78494092a326 | 95 | FingerPosition(1000,2400,2400, 600, 2400); //Open |
julioefajardo | 4:d8fd3c4484cc | 96 | } |
julioefajardo | 4:d8fd3c4484cc | 97 | } |
julioefajardo | 0:f2b89c6a8a16 | 98 | } |
julioefajardo | 0:f2b89c6a8a16 | 99 | COCO = 0; |
julioefajardo | 0:f2b89c6a8a16 | 100 | } |
julioefajardo | 0:f2b89c6a8a16 | 101 | } |
julioefajardo | 0:f2b89c6a8a16 | 102 | } |
julioefajardo | 4:d8fd3c4484cc | 103 | |
julioefajardo | 5:49c5553b6e2c | 104 | //EMG sampler Ts = 1ms |
julioefajardo | 4:d8fd3c4484cc | 105 | void ADC_Sampler() { |
julioefajardo | 4:d8fd3c4484cc | 106 | EMG1 = (E1.read()-Ref.read())*3.3f; |
julioefajardo | 4:d8fd3c4484cc | 107 | EMG2 = (E2.read()-Ref.read())*3.3f; |
julioefajardo | 4:d8fd3c4484cc | 108 | EMG3 = (E3.read()-Ref.read())*3.3f; |
julioefajardo | 6:78494092a326 | 109 | Serial_Oscilloscope(FALSE,RAW); |
julioefajardo | 4:d8fd3c4484cc | 110 | uint32_t m = __get_PRIMASK(); |
julioefajardo | 4:d8fd3c4484cc | 111 | __disable_irq(); |
julioefajardo | 4:d8fd3c4484cc | 112 | for(int j=24;j>0;j--) { |
julioefajardo | 4:d8fd3c4484cc | 113 | samples[j]=samples[j-1]; //Fill Array |
julioefajardo | 4:d8fd3c4484cc | 114 | samples2[j]=samples2[j-1]; //Fill Array |
julioefajardo | 4:d8fd3c4484cc | 115 | samples3[j]=samples3[j-1]; //Fill Array |
julioefajardo | 4:d8fd3c4484cc | 116 | } |
julioefajardo | 4:d8fd3c4484cc | 117 | samples[0]=EMG1; |
julioefajardo | 4:d8fd3c4484cc | 118 | samples2[0]=EMG2; |
julioefajardo | 4:d8fd3c4484cc | 119 | samples3[0]=EMG3; |
julioefajardo | 4:d8fd3c4484cc | 120 | __set_PRIMASK(m); |
julioefajardo | 4:d8fd3c4484cc | 121 | COCO = 1; |
julioefajardo | 4:d8fd3c4484cc | 122 | } |
julioefajardo | 4:d8fd3c4484cc | 123 | |
julioefajardo | 5:49c5553b6e2c | 124 | //action selection trough serial console |
julioefajardo | 5:49c5553b6e2c | 125 | void RX_Interrupt(void){ |
julioefajardo | 5:49c5553b6e2c | 126 | action = pc.getc(); |
julioefajardo | 5:49c5553b6e2c | 127 | /*switch(action){ |
julioefajardo | 5:49c5553b6e2c | 128 | case '1': pc.printf("Power Grip\n"); break; |
julioefajardo | 5:49c5553b6e2c | 129 | case '2': pc.printf("Point\n"); break; |
julioefajardo | 5:49c5553b6e2c | 130 | case '3': pc.printf("Pinch\n"); break; |
julioefajardo | 5:49c5553b6e2c | 131 | case '4': pc.printf("Lateral\n"); break; |
julioefajardo | 5:49c5553b6e2c | 132 | case '5': pc.printf("Tripod\n"); break; |
julioefajardo | 5:49c5553b6e2c | 133 | default: pc.printf("Power Grip\n"); |
julioefajardo | 5:49c5553b6e2c | 134 | }*/ |
julioefajardo | 4:d8fd3c4484cc | 135 | } |
julioefajardo | 4:d8fd3c4484cc | 136 | |
julioefajardo | 5:49c5553b6e2c | 137 | //PWM initialization for servos |
julioefajardo | 5:49c5553b6e2c | 138 | void PWM_Init(void){ |
julioefajardo | 5:49c5553b6e2c | 139 | //Open -> 0.6ms - Close 2.4ms |
julioefajardo | 6:78494092a326 | 140 | Thumb.period(0.02f); |
julioefajardo | 6:78494092a326 | 141 | Thumb.pulsewidth(0.0010f); |
julioefajardo | 5:49c5553b6e2c | 142 | //Open -> 2.4ms - Close 0.6ms |
julioefajardo | 6:78494092a326 | 143 | //Index.period(0.02f); |
julioefajardo | 6:78494092a326 | 144 | Index.pulsewidth(0.0024f); |
julioefajardo | 5:49c5553b6e2c | 145 | //Open -> 2.4ms - Close 0.6ms |
julioefajardo | 6:78494092a326 | 146 | Middle.period(0.02f); |
julioefajardo | 6:78494092a326 | 147 | Middle.pulsewidth(0.0024f); |
julioefajardo | 5:49c5553b6e2c | 148 | //Open -> 0.6ms - Close 2.4ms |
julioefajardo | 6:78494092a326 | 149 | //Pinky.period(0.02f); |
julioefajardo | 6:78494092a326 | 150 | Pinky.pulsewidth(0.0006f); |
julioefajardo | 5:49c5553b6e2c | 151 | //Open -> 0.6ms - Close 2.4ms |
julioefajardo | 6:78494092a326 | 152 | ThumbRot.pulsewidth(0.0006f); |
julioefajardo | 5:49c5553b6e2c | 153 | } |
julioefajardo | 5:49c5553b6e2c | 154 | |
julioefajardo | 5:49c5553b6e2c | 155 | //send data through serail port for oscilloscope visualization |
julioefajardo | 5:49c5553b6e2c | 156 | void Serial_Oscilloscope(uint8_t activation, uint8_t type){ |
julioefajardo | 5:49c5553b6e2c | 157 | if (activation){ |
julioefajardo | 5:49c5553b6e2c | 158 | switch(type){ |
julioefajardo | 6:78494092a326 | 159 | case RAW: pc.printf("%.10f,%.10f\n\r",EMG1,EMG2); break; |
julioefajardo | 6:78494092a326 | 160 | case RECTIFIED: pc.printf("%.10f,%.10f\n\r",abs_output[0],abs_output2[0]);break; |
julioefajardo | 6:78494092a326 | 161 | case SMOOTH: pc.printf("%.10f,%.10f\n\r",mean,mean2); break; |
julioefajardo | 6:78494092a326 | 162 | default: pc.printf("%.10f,%.10f\n\r",EMG1,EMG2); |
julioefajardo | 5:49c5553b6e2c | 163 | } |
julioefajardo | 5:49c5553b6e2c | 164 | } |
julioefajardo | 5:49c5553b6e2c | 165 | } |
julioefajardo | 5:49c5553b6e2c | 166 | |
julioefajardo | 5:49c5553b6e2c | 167 | //finger position through servos - values on microseconds (600 us - 2400 us) |
julioefajardo | 5:49c5553b6e2c | 168 | void FingerPosition(float32_t thumb_us, float32_t index_us, float32_t middle_us, float32_t pinky_us, float32_t thumbrot_us){ |
julioefajardo | 6:78494092a326 | 169 | Thumb.pulsewidth(thumb_us/1000000.0f); |
julioefajardo | 6:78494092a326 | 170 | Index.pulsewidth(index_us/1000000.0f); |
julioefajardo | 6:78494092a326 | 171 | Middle.pulsewidth(middle_us/1000000.0f); |
julioefajardo | 6:78494092a326 | 172 | Pinky.pulsewidth(pinky_us/1000000.0f); |
julioefajardo | 6:78494092a326 | 173 | ThumbRot.pulsewidth(thumbrot_us/1000000.0f); |
julioefajardo | 5:49c5553b6e2c | 174 | } |