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@5:49c5553b6e2c, 2015-10-11 (annotated)

Committer:
julioefajardo
Date:
Sun Oct 11 03:42:30 2015 +0000
Revision:
5:49c5553b6e2c
Parent:
4:d8fd3c4484cc
Child:
6:78494092a326
Version 1.3; EMG Galieo Hand Prosthesis Controller ; Refactorization

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