Module 9 Super Team / EMG1

Changed some stuff

Fork of EMG by Kevin Hetterscheid

emg.cpp@29:373a2facb3ae, 2015-10-29 (annotated)

Committer:
AeroKev
Date:
Thu Oct 29 10:53:46 2015 +0000
Revision:
29:373a2facb3ae
Parent:
28:5a12ce2fa441
Child:
30:aa0389e04d47
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vsluiter 0:32bb76391d89 1 #include "mbed.h"
vsluiter 11:ce72ec658a95 2 #include "HIDScope.h"
AeroKev 24:b7b3e87e0687 3 #define MOV_AVG_NUM 100
dubbie 28:5a12ce2fa441 4
vsluiter 4:8b298dfada81 5 //Define objects
AeroKev 24:b7b3e87e0687 6 AnalogIn b_emg(A0); //Analog input bicep
AeroKev 24:b7b3e87e0687 7 AnalogIn t_emg(A1); //Analog input tricep
AeroKev 24:b7b3e87e0687 8 AnalogIn p_emg(A2); //Analog input bicep for push
AeroKev 18:e753220c7ba6 9
AeroKev 24:b7b3e87e0687 10 double b_highV[4];
AeroKev 24:b7b3e87e0687 11 double b_lowV[4];
AeroKev 24:b7b3e87e0687 12 double t_highV[4];
AeroKev 24:b7b3e87e0687 13 double t_lowV[4];
AeroKev 24:b7b3e87e0687 14 double p_highV[4];
AeroKev 24:b7b3e87e0687 15 double p_lowV[4];
AeroKev 29:373a2facb3ae 16 double b_min = 10.0f;
AeroKev 29:373a2facb3ae 17 double t_min = 10.0f;
AeroKev 29:373a2facb3ae 18 double p_min = 10.0f;
AeroKev 24:b7b3e87e0687 19
dubbie 28:5a12ce2fa441 20 void emg_cal(int emg){
AeroKev 29:373a2facb3ae 21 double cur;
dubbie 28:5a12ce2fa441 22 if(emg == 1){
dubbie 28:5a12ce2fa441 23 for(int i=0; i<300; i++){
AeroKev 29:373a2facb3ae 24 cur = b_emg.read();
AeroKev 29:373a2facb3ae 25 if(cur < b_min)
AeroKev 29:373a2facb3ae 26 b_min = cur;
dubbie 28:5a12ce2fa441 27 }
dubbie 28:5a12ce2fa441 28 }
AeroKev 29:373a2facb3ae 29 else if(emg == 2){
dubbie 28:5a12ce2fa441 30 for(int i=0; i<300; i++){
AeroKev 29:373a2facb3ae 31 cur = t_emg.read();
AeroKev 29:373a2facb3ae 32 if(cur < t_min)
AeroKev 29:373a2facb3ae 33 t_min = cur;
dubbie 28:5a12ce2fa441 34 }
AeroKev 29:373a2facb3ae 35 }
AeroKev 29:373a2facb3ae 36 else if(emg == 3){
AeroKev 29:373a2facb3ae 37 for(int i=0; i<300; i++){
AeroKev 29:373a2facb3ae 38 cur = p_emg.read();
AeroKev 29:373a2facb3ae 39 if(cur < p_min)
AeroKev 29:373a2facb3ae 40 p_min = cur;
AeroKev 29:373a2facb3ae 41 }
dubbie 28:5a12ce2fa441 42 }
dubbie 28:5a12ce2fa441 43 }
AeroKev 18:e753220c7ba6 44 double filter(double input, double coeff_input[], double coeff_output[], double prev_outputs[])
AeroKev 18:e753220c7ba6 45 {
AeroKev 18:e753220c7ba6 46 double new_input = input;
AeroKev 18:e753220c7ba6 47 for(int i=1; i<5; i++)
AeroKev 18:e753220c7ba6 48 new_input -= coeff_input[i] * prev_outputs[i-1];
AeroKev 18:e753220c7ba6 49
AeroKev 18:e753220c7ba6 50 double new_output = coeff_output[0] * new_input;
AeroKev 18:e753220c7ba6 51 for(int i=1; i<5; i++)
AeroKev 18:e753220c7ba6 52 new_output += coeff_output[i] * prev_outputs[i-1];
AeroKev 18:e753220c7ba6 53
AeroKev 18:e753220c7ba6 54 // Set the new output as the first value of the 'recent outputs'
AeroKev 18:e753220c7ba6 55 for(int i = 3; i > 0; i--)
AeroKev 18:e753220c7ba6 56 prev_outputs[i] = prev_outputs[i-1];
AeroKev 18:e753220c7ba6 57 prev_outputs[0] = new_input;
AeroKev 18:e753220c7ba6 58 return new_output;
AeroKev 18:e753220c7ba6 59 }
AeroKev 18:e753220c7ba6 60 double fh_b[]= {0.7602, -3.0406, 4.5609, -3.0406, 0.7602};
AeroKev 18:e753220c7ba6 61 double fh_a[]= {1, -3.4532, 4.5041, -2.6273, 0.5778};
AeroKev 24:b7b3e87e0687 62 double b_highpass_filter(double u)
AeroKev 24:b7b3e87e0687 63 {
AeroKev 24:b7b3e87e0687 64 return filter(u, fh_a, fh_b, t_highV);
AeroKev 24:b7b3e87e0687 65 }
AeroKev 24:b7b3e87e0687 66 double t_highpass_filter(double u)
AeroKev 18:e753220c7ba6 67 {
AeroKev 24:b7b3e87e0687 68 return filter(u, fh_a, fh_b, b_highV);
AeroKev 24:b7b3e87e0687 69 }
AeroKev 24:b7b3e87e0687 70 double p_highpass_filter(double u)
AeroKev 24:b7b3e87e0687 71 {
AeroKev 24:b7b3e87e0687 72 return filter(u, fh_a, fh_b, p_highV);
AeroKev 18:e753220c7ba6 73 }
AeroKev 18:e753220c7ba6 74
AeroKev 21:49362a17495b 75 double fl_b[]= {0.00001329, 0.00005317, 0.00007976, 0.00005317, 0.00001329};
AeroKev 21:49362a17495b 76 double fl_a[]= {1.0000, -3.6717, 5.0680, -3.1160, 0.7199};
AeroKev 24:b7b3e87e0687 77 double b_lowpass_filter(double u)
AeroKev 24:b7b3e87e0687 78 {
AeroKev 24:b7b3e87e0687 79 return filter(u, fl_a, fl_b, t_lowV);
AeroKev 24:b7b3e87e0687 80 }
AeroKev 24:b7b3e87e0687 81 double t_lowpass_filter(double u)
AeroKev 18:e753220c7ba6 82 {
AeroKev 24:b7b3e87e0687 83
AeroKev 24:b7b3e87e0687 84 return filter(u, fl_a, fl_b, b_lowV);
AeroKev 24:b7b3e87e0687 85 }
AeroKev 24:b7b3e87e0687 86 double p_lowpass_filter(double u)
AeroKev 24:b7b3e87e0687 87 {
AeroKev 24:b7b3e87e0687 88
AeroKev 24:b7b3e87e0687 89 return filter(u, fl_a, fl_b, p_lowV);
AeroKev 18:e753220c7ba6 90 }
vsluiter 2:e314bb3b2d99 91
AeroKev 24:b7b3e87e0687 92 double last_biceps = 0;
AeroKev 24:b7b3e87e0687 93 double last_triceps = 0;
AeroKev 24:b7b3e87e0687 94 double last_push = 0;
AeroKev 21:49362a17495b 95 /** Sample function
AeroKev 21:49362a17495b 96 * this function samples the emg and sends it to HIDScope
AeroKev 21:49362a17495b 97 **/
AeroKev 24:b7b3e87e0687 98 void sample(double& bicout, double& tricout, double& pushout)
vsluiter 2:e314bb3b2d99 99 {
AeroKev 24:b7b3e87e0687 100 double b_input = b_emg.read();
AeroKev 24:b7b3e87e0687 101 double output1 = b_highpass_filter(b_input);
AeroKev 18:e753220c7ba6 102 double output2 = fabs(output1);
AeroKev 24:b7b3e87e0687 103 double output3 = b_lowpass_filter(output2);
AeroKev 24:b7b3e87e0687 104
AeroKev 24:b7b3e87e0687 105 double t_input = t_emg.read();
AeroKev 24:b7b3e87e0687 106 double output4 = t_highpass_filter(t_input);
AeroKev 24:b7b3e87e0687 107 double output5 = fabs(output4);
AeroKev 24:b7b3e87e0687 108 double output6 = t_lowpass_filter(output5);
AeroKev 24:b7b3e87e0687 109
AeroKev 24:b7b3e87e0687 110 double p_input = p_emg.read();
AeroKev 24:b7b3e87e0687 111 double output7 = p_highpass_filter(p_input);
AeroKev 24:b7b3e87e0687 112 double output8 = fabs(output7);
AeroKev 24:b7b3e87e0687 113 double output9 = p_lowpass_filter(output8);
AeroKev 24:b7b3e87e0687 114
AeroKev 24:b7b3e87e0687 115 /*double tot = output3;
AeroKev 24:b7b3e87e0687 116 for(int i=0; i<MOV_AVG_NUM-1; i++) {
AeroKev 24:b7b3e87e0687 117 tot += lastOutputs[i];
AeroKev 24:b7b3e87e0687 118 if(i != 0) lastOutputs[i] = lastOutputs[i-1];
AeroKev 24:b7b3e87e0687 119 }
AeroKev 24:b7b3e87e0687 120 lastOutputs[0] = output3;
AeroKev 24:b7b3e87e0687 121 output3 = tot/MOV_AVG_NUM;*/
AeroKev 18:e753220c7ba6 122
AeroKev 24:b7b3e87e0687 123 /* Second, set the sampled emg value in channel zero (the first channel) in the 'HIDScope' variable named 'scope' */
AeroKev 24:b7b3e87e0687 124
dubbie 28:5a12ce2fa441 125 if (last_biceps == 0 and output3 > b_min+0.2*b_min) {
AeroKev 24:b7b3e87e0687 126 bicout = 1;
dubbie 28:5a12ce2fa441 127 } else if (last_biceps == 1 and output3 < b_min-0.2*b_min) {
AeroKev 24:b7b3e87e0687 128 bicout = 0;
AeroKev 24:b7b3e87e0687 129 } else {
AeroKev 24:b7b3e87e0687 130 bicout = last_biceps;
AeroKev 24:b7b3e87e0687 131 }
AeroKev 24:b7b3e87e0687 132 last_biceps = bicout;
AeroKev 24:b7b3e87e0687 133
AeroKev 24:b7b3e87e0687 134 /*if (output3>0.06) {//This is the output for the right bicep
AeroKev 24:b7b3e87e0687 135 bicout=1;
AeroKev 24:b7b3e87e0687 136 } else if(output3>0.04) {
AeroKev 24:b7b3e87e0687 137 bicout=0.5;
AeroKev 24:b7b3e87e0687 138 } else {
AeroKev 24:b7b3e87e0687 139 bicout=0;
AeroKev 24:b7b3e87e0687 140 }*/
AeroKev 23:8647e71ca713 141
AeroKev 24:b7b3e87e0687 142 /*if (output6>0.0561) {//this is the output for the right tricep
AeroKev 24:b7b3e87e0687 143 tricout=1;
AeroKev 24:b7b3e87e0687 144 } else if(output6>0.0380) {
AeroKev 24:b7b3e87e0687 145 tricout=0.5;
AeroKev 24:b7b3e87e0687 146 } else {
AeroKev 24:b7b3e87e0687 147 tricout=0;
AeroKev 24:b7b3e87e0687 148 }*/
AeroKev 24:b7b3e87e0687 149
dubbie 28:5a12ce2fa441 150 if (last_triceps == 0 and output6 > t_min+0.1*t_min) {
AeroKev 24:b7b3e87e0687 151 tricout = 1;
dubbie 28:5a12ce2fa441 152 } else if(last_triceps == 1 and output6 < t_min-0.01*t_min) {
AeroKev 24:b7b3e87e0687 153 tricout = 0;
AeroKev 24:b7b3e87e0687 154 } else {
AeroKev 24:b7b3e87e0687 155 tricout = last_triceps;
AeroKev 24:b7b3e87e0687 156 }
AeroKev 24:b7b3e87e0687 157 last_triceps = tricout;
AeroKev 24:b7b3e87e0687 158
dubbie 28:5a12ce2fa441 159 if (last_push ==0 and output9>p_min+0.2*p_min) {//this is the output for the left bicep (the push motion)
AeroKev 24:b7b3e87e0687 160 pushout=1;
dubbie 28:5a12ce2fa441 161 } else if(last_triceps == 1 and output9<b_min-0.2*b_min) {
dubbie 25:6d1b035f4838 162 pushout=0;
AeroKev 24:b7b3e87e0687 163 } else {
dubbie 25:6d1b035f4838 164 pushout = last_push;
AeroKev 24:b7b3e87e0687 165 }
dubbie 25:6d1b035f4838 166 last_push = pushout;
AeroKev 21:49362a17495b 167 }