Jesse Kaiser / Mbed 2 deprecated Lampje_EMG_Gr6

Aansturen van een LED script dmv twee EMG signalen

Dependencies:   HIDScope MODSERIAL mbed-dsp mbed

main.cpp@7:119b85a92a09, 2014-10-17 (annotated)

Committer:
jessekaiser
Date:
Fri Oct 17 13:41:56 2014 +0000
Revision:
7:119b85a92a09
Parent:
6:8f4138a811e0
Child:
8:ee16c139e10b
Met 2 EMG signalen licht aansturen. Inclusief Notch filter

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jessekaiser 0:db396b9f4b4c 1 #include "mbed.h"
jessekaiser 0:db396b9f4b4c 2 #include "HIDScope.h"
jessekaiser 0:db396b9f4b4c 3 #include "arm_math.h"
jessekaiser 0:db396b9f4b4c 4 #include "MODSERIAL.h"
jessekaiser 0:db396b9f4b4c 5
jessekaiser 0:db396b9f4b4c 6 Serial pc(USBTX, USBRX); // tx, rx
jessekaiser 0:db396b9f4b4c 7 DigitalOut myled1(LED_RED);
jessekaiser 0:db396b9f4b4c 8 DigitalOut myled2(LED_GREEN);
jessekaiser 0:db396b9f4b4c 9 DigitalOut myled3(LED_BLUE);
jessekaiser 1:099b19376f16 10 PwmOut motorsignal(PTD4);
jessekaiser 0:db396b9f4b4c 11
jessekaiser 0:db396b9f4b4c 12 //Define objects
jessekaiser 3:0895fa0a6ca4 13 AnalogIn emg0(PTB1); //Analog input
jessekaiser 2:39e1bde54e73 14 AnalogIn emg1(PTB2); //Analog input
jessekaiser 3:0895fa0a6ca4 15 HIDScope scope(4);
jessekaiser 0:db396b9f4b4c 16
jessekaiser 0:db396b9f4b4c 17 arm_biquad_casd_df1_inst_f32 lowpass;
jessekaiser 2:39e1bde54e73 18 //lowpass filter settings: Fc = 225 Hz, Fs = 500 Hz, Gain = -3 dB
jessekaiser 0:db396b9f4b4c 19 float lowpass_const[] = {0.8005910266528647, 1.6011820533057295, 0.8005910266528647, -1.5610153912536877, -0.6413487153577715};
jessekaiser 0:db396b9f4b4c 20 //state values
jessekaiser 0:db396b9f4b4c 21 float lowpass_states[4];
jessekaiser 0:db396b9f4b4c 22 arm_biquad_casd_df1_inst_f32 highpass;
jessekaiser 2:39e1bde54e73 23 //highpass filter settings: Fc = 20 Hz, Fs = 500 Hz, Gain = -3 dB
jessekaiser 0:db396b9f4b4c 24 float highpass_const[] = {0.956542835577484, -1.913085671154968, 0.956542835577484, 1.911196288237583, -0.914975054072353};
jessekaiser 0:db396b9f4b4c 25 //state values
jessekaiser 0:db396b9f4b4c 26 float highpass_states[4];
jessekaiser 7:119b85a92a09 27 arm_biquad_casd_df1_inst_f32 notch;
jessekaiser 7:119b85a92a09 28 //highpass filter settings: Fc = 20 Hz, Fs = 500 Hz, Gain = -3 dB
jessekaiser 7:119b85a92a09 29 float notch_const[] = {0.7063988100714527, -1.1429772843080923, 0.7063988100714527, 1.1429772843080923, -0.41279762014290533};
jessekaiser 7:119b85a92a09 30 //state values
jessekaiser 7:119b85a92a09 31 float notch_states[4];
jessekaiser 2:39e1bde54e73 32
jessekaiser 2:39e1bde54e73 33 //De globale variabele voor het gefilterde EMG signaal
jessekaiser 2:39e1bde54e73 34 float filtered_biceps;
jessekaiser 2:39e1bde54e73 35 float filtered_deltoid;
jessekaiser 0:db396b9f4b4c 36
jessekaiser 0:db396b9f4b4c 37
jessekaiser 0:db396b9f4b4c 38 /** Looper function
jessekaiser 0:db396b9f4b4c 39 * functions used for Ticker and Timeout should be of type void <name>(void)
jessekaiser 0:db396b9f4b4c 40 * i.e. no input arguments, no output arguments.
jessekaiser 0:db396b9f4b4c 41 * if you want to change a variable that you use in other places (for example in main)
jessekaiser 0:db396b9f4b4c 42 * you will have to make that variable global in order to be able to reach it both from
jessekaiser 0:db396b9f4b4c 43 * the function called at interrupt time, and in the main function.
jessekaiser 0:db396b9f4b4c 44 * To make a variable global, define it under the includes.
jessekaiser 0:db396b9f4b4c 45 * variables that are changed in the interrupt routine (written to) should be made
jessekaiser 0:db396b9f4b4c 46 * 'volatile' to let the compiler know that those values may change outside the current context.
jessekaiser 0:db396b9f4b4c 47 * i.e.: "volatile uint16_t emg_value;" instead of "uint16_t emg_value"
jessekaiser 0:db396b9f4b4c 48 * in the example below, the variable is not re-used in the main function, and is thus declared
jessekaiser 0:db396b9f4b4c 49 * local in the looper function only.
jessekaiser 0:db396b9f4b4c 50 **/
jessekaiser 1:099b19376f16 51
jessekaiser 1:099b19376f16 52
jessekaiser 0:db396b9f4b4c 53 void looper()
jessekaiser 0:db396b9f4b4c 54 {
jessekaiser 1:099b19376f16 55 /*variable to store value in*/
jessekaiser 2:39e1bde54e73 56 uint16_t emg_value1;
jessekaiser 3:0895fa0a6ca4 57 uint16_t emg_value2;
jessekaiser 1:099b19376f16 58
jessekaiser 2:39e1bde54e73 59 float emg_value1_f32;
jessekaiser 2:39e1bde54e73 60 float emg_value2_f32;
jessekaiser 0:db396b9f4b4c 61 /*put raw emg value both in red and in emg_value*/
jessekaiser 2:39e1bde54e73 62 emg_value1 = emg0.read_u16(); // read direct ADC result, converted to 16 bit integer (0..2^16 = 0..65536 = 0..3.3V)
jessekaiser 2:39e1bde54e73 63 emg_value1_f32 = emg0.read();
jessekaiser 3:0895fa0a6ca4 64
jessekaiser 2:39e1bde54e73 65 emg_value2 = emg1.read_u16();
jessekaiser 2:39e1bde54e73 66 emg_value2_f32 = emg1.read();
jessekaiser 0:db396b9f4b4c 67
jessekaiser 2:39e1bde54e73 68 //process emg biceps
jessekaiser 2:39e1bde54e73 69 arm_biquad_cascade_df1_f32(&highpass, &emg_value1_f32, &filtered_biceps, 1 );
jessekaiser 2:39e1bde54e73 70 filtered_biceps = fabs(filtered_biceps);
jessekaiser 7:119b85a92a09 71 arm_biquad_cascade_df1_f32(&notch, &filtered_biceps, &filtered_biceps, 1 );
jessekaiser 2:39e1bde54e73 72 arm_biquad_cascade_df1_f32(&lowpass, &filtered_biceps, &filtered_biceps, 1 );
jessekaiser 3:0895fa0a6ca4 73
jessekaiser 2:39e1bde54e73 74 //process emg deltoid
jessekaiser 2:39e1bde54e73 75 arm_biquad_cascade_df1_f32(&highpass, &emg_value2_f32, &filtered_deltoid, 1 );
jessekaiser 3:0895fa0a6ca4 76 filtered_deltoid = fabs(filtered_deltoid);
jessekaiser 7:119b85a92a09 77 arm_biquad_cascade_df1_f32(&notch, &filtered_deltoid, &filtered_deltoid, 1 );
jessekaiser 2:39e1bde54e73 78 arm_biquad_cascade_df1_f32(&lowpass, &filtered_deltoid, &filtered_deltoid, 1 );
jessekaiser 1:099b19376f16 79
jessekaiser 0:db396b9f4b4c 80 /*send value to PC. */
jessekaiser 3:0895fa0a6ca4 81 scope.set(0,emg_value1); //Raw EMG signal biceps
jessekaiser 3:0895fa0a6ca4 82 scope.set(1,emg_value2); //Raw EMG signal Deltoid
jessekaiser 3:0895fa0a6ca4 83 scope.set(2,filtered_biceps); //processed float biceps
jessekaiser 3:0895fa0a6ca4 84 scope.set(3,filtered_deltoid); //processed float deltoid
jessekaiser 0:db396b9f4b4c 85 scope.send();
jessekaiser 0:db396b9f4b4c 86
jessekaiser 0:db396b9f4b4c 87 }
jessekaiser 0:db396b9f4b4c 88
jessekaiser 0:db396b9f4b4c 89 void BlinkRed(int n)
jessekaiser 0:db396b9f4b4c 90 {
jessekaiser 0:db396b9f4b4c 91 for (int i=0; i<n; i++) {
jessekaiser 0:db396b9f4b4c 92 myled1 = 1;
jessekaiser 0:db396b9f4b4c 93 myled2 = 1;
jessekaiser 0:db396b9f4b4c 94 myled3 = 1;
jessekaiser 3:0895fa0a6ca4 95 wait(0.1);
jessekaiser 0:db396b9f4b4c 96 myled1 = 0;
jessekaiser 0:db396b9f4b4c 97 myled2 = 1;
jessekaiser 0:db396b9f4b4c 98 myled3 = 1;
jessekaiser 3:0895fa0a6ca4 99 wait(0.1);
jessekaiser 0:db396b9f4b4c 100 }
jessekaiser 0:db396b9f4b4c 101 }
jessekaiser 0:db396b9f4b4c 102
jessekaiser 3:0895fa0a6ca4 103 void BlinkGreen ()
jessekaiser 3:0895fa0a6ca4 104 {
jessekaiser 3:0895fa0a6ca4 105
jessekaiser 3:0895fa0a6ca4 106 myled1 = 1;
jessekaiser 3:0895fa0a6ca4 107 myled2 = 1;
jessekaiser 3:0895fa0a6ca4 108 myled3 = 1;
jessekaiser 3:0895fa0a6ca4 109 wait(0.1);
jessekaiser 3:0895fa0a6ca4 110 myled1 = 1;
jessekaiser 3:0895fa0a6ca4 111 myled2 = 0;
jessekaiser 3:0895fa0a6ca4 112 myled3 = 1;
jessekaiser 3:0895fa0a6ca4 113 wait(0.1);
jessekaiser 3:0895fa0a6ca4 114 }
jessekaiser 3:0895fa0a6ca4 115
jessekaiser 3:0895fa0a6ca4 116
jessekaiser 3:0895fa0a6ca4 117 void BlinkBlue(int n)
jessekaiser 2:39e1bde54e73 118 {
jessekaiser 2:39e1bde54e73 119 for (int i=0; i<n; i++) {
jessekaiser 2:39e1bde54e73 120 myled1 = 1;
jessekaiser 2:39e1bde54e73 121 myled2 = 1;
jessekaiser 2:39e1bde54e73 122 myled3 = 1;
jessekaiser 3:0895fa0a6ca4 123 wait(0.1);
jessekaiser 2:39e1bde54e73 124 myled1 = 1;
jessekaiser 3:0895fa0a6ca4 125 myled2 = 1;
jessekaiser 3:0895fa0a6ca4 126 myled3 = 0;
jessekaiser 3:0895fa0a6ca4 127 wait(0.1);
jessekaiser 2:39e1bde54e73 128 }
jessekaiser 2:39e1bde54e73 129 }
jessekaiser 2:39e1bde54e73 130
jessekaiser 0:db396b9f4b4c 131 int main()
jessekaiser 1:099b19376f16 132 {
jessekaiser 0:db396b9f4b4c 133 pc.baud(115200);
jessekaiser 1:099b19376f16 134
jessekaiser 0:db396b9f4b4c 135 Ticker log_timer;
jessekaiser 1:099b19376f16 136 //set up filters. Use external array for constants
jessekaiser 0:db396b9f4b4c 137 arm_biquad_cascade_df1_init_f32(&lowpass,1 , lowpass_const, lowpass_states);
jessekaiser 0:db396b9f4b4c 138 arm_biquad_cascade_df1_init_f32(&highpass,1 ,highpass_const,highpass_states);
jessekaiser 7:119b85a92a09 139 arm_biquad_cascade_df1_init_f32(&notch,1 , notch_const, notch_states);
jessekaiser 0:db396b9f4b4c 140
jessekaiser 0:db396b9f4b4c 141 /**Here you attach the 'void looper(void)' function to the Ticker object
jessekaiser 0:db396b9f4b4c 142 * The looper() function will be called every 0.01 seconds.
jessekaiser 0:db396b9f4b4c 143 * Please mind that the parentheses after looper are omitted when using attach.
jessekaiser 0:db396b9f4b4c 144 */
jessekaiser 0:db396b9f4b4c 145 log_timer.attach(looper, 0.001);
jessekaiser 1:099b19376f16 146 while(1) { //Loop
jessekaiser 1:099b19376f16 147 /*Empty!*/
jessekaiser 1:099b19376f16 148 /*Everything is handled by the interrupt routine now!*/
jessekaiser 1:099b19376f16 149 {
jessekaiser 6:8f4138a811e0 150
jessekaiser 1:099b19376f16 151 while(1) {
jessekaiser 3:0895fa0a6ca4 152 pc.printf("Span de biceps aan om het instellen te starten");
jessekaiser 1:099b19376f16 153 do {
jessekaiser 3:0895fa0a6ca4 154 BlinkGreen();
jessekaiser 1:099b19376f16 155 }
jessekaiser 3:0895fa0a6ca4 156 while(filtered_biceps < 0.04);
jessekaiser 3:0895fa0a6ca4 157 while(filtered_biceps > 0.04) { //Wanneer het EMG signaal een piek geeft wordt het volgende uitgevoerd.
jessekaiser 4:40aa1fb60a24 158 BlinkRed(20);
jessekaiser 6:8f4138a811e0 159 if (filtered_deltoid > 0.04) {
jessekaiser 5:03105d8bf72c 160 BlinkBlue(20);
jessekaiser 4:40aa1fb60a24 161 } else if (filtered_biceps < 0.04 && filtered_deltoid < 0.04) {
jessekaiser 3:0895fa0a6ca4 162 break;
jessekaiser 3:0895fa0a6ca4 163 }
jessekaiser 0:db396b9f4b4c 164
jessekaiser 6:8f4138a811e0 165 if(filtered_biceps > 0.04) {
jessekaiser 3:0895fa0a6ca4 166 myled1 = 1;
jessekaiser 3:0895fa0a6ca4 167 myled2 = 0;
jessekaiser 3:0895fa0a6ca4 168 myled3 = 1;
jessekaiser 3:0895fa0a6ca4 169 wait(5); //Dit wordt de duur van het slaan en teruggaan naar de beginpositie totdat er opnieuw gemeten kan worden.
jessekaiser 6:8f4138a811e0 170
jessekaiser 3:0895fa0a6ca4 171 }
jessekaiser 0:db396b9f4b4c 172 }
jessekaiser 0:db396b9f4b4c 173 }
jessekaiser 0:db396b9f4b4c 174 }
jessekaiser 0:db396b9f4b4c 175 }
jessekaiser 1:099b19376f16 176 }