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@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?

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 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 }