Biorobotica TIC / Mbed 2 deprecated DEMO_TEST_LIJN

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   Demo_TEST3 QEI biquadFilter mbed

Fork of DEMO_TEST_LIJN by Tim Luten

main.cpp@4:5ceb8f058874, 2018-10-24 (annotated)

Committer:
Hubertus
Date:
Wed Oct 24 11:42:35 2018 +0000
Revision:
4:5ceb8f058874
Parent:
3:be5ac89a0b53
Child:
5:e7253884c2e4
Child:
6:1f722bf6a89b
3x EMG Signaal gefilterd en wel + hidscope

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Hubertus 0:df553b18547d 1 #include "mbed.h"
Hubertus 0:df553b18547d 2 #include "math.h"
Hubertus 0:df553b18547d 3 #include "BiQuad.h"
Hubertus 4:5ceb8f058874 4 //#include "Servo.h"
Hubertus 4:5ceb8f058874 5 #include "HIDScope.h"
Hubertus 0:df553b18547d 6
Hubertus 1:5c3259ecf10a 7
Hubertus 3:be5ac89a0b53 8
Hubertus 0:df553b18547d 9 //Define objects
Hubertus 4:5ceb8f058874 10 AnalogIn emgL(A0); // EMG Left Arm
Hubertus 4:5ceb8f058874 11 AnalogIn emgR(A1); // EMG Right Arm
Hubertus 4:5ceb8f058874 12 AnalogIn emgS(A2); // EMG Servo spier
Hubertus 0:df553b18547d 13 DigitalOut ledB(LED_BLUE); // Informative LED for gaining zero and max
Hubertus 1:5c3259ecf10a 14 DigitalOut ledR(LED_RED);
Hubertus 0:df553b18547d 15 DigitalOut ledG(LED_GREEN); // Informative LED for gaining zero and max
Hubertus 4:5ceb8f058874 16 DigitalIn CalButton(PTA4); // Button used for gaining zero and max
Hubertus 4:5ceb8f058874 17 DigitalIn zeromax(PTC6); // Button used for switching between zero and max
Hubertus 0:df553b18547d 18 Ticker emgSampleTicker; // Ticker for sample frequency
Hubertus 4:5ceb8f058874 19 HIDScope scope( 6 );
Hubertus 4:5ceb8f058874 20 //Servo myservo(D13);
Hubertus 3:be5ac89a0b53 21 DigitalOut motor1direction(D7);
Hubertus 3:be5ac89a0b53 22 PwmOut motor1control(D6);
Hubertus 0:df553b18547d 23
Hubertus 0:df553b18547d 24
Hubertus 4:5ceb8f058874 25 int P= 200; // Number of points for movag first emg
Hubertus 4:5ceb8f058874 26 int Q = 200; // Number of points for movag second emg
Hubertus 4:5ceb8f058874 27 int R = 200; // Number of points for movag third emg
Hubertus 0:df553b18547d 28 double A[200]; // Vector for storing data of first emg
Hubertus 0:df553b18547d 29 double B[200]; // Vector for storing data of second emg
Hubertus 4:5ceb8f058874 30 double C[200]; // Vector for storing data of third emg
Hubertus 0:df553b18547d 31 int k = 0; // Counter for configuration:
Hubertus 4:5ceb8f058874 32 double Lvector[200]; // Vector for Rwaarde configuration
Hubertus 4:5ceb8f058874 33 double Lwaarde[2]; // Vector for storage of max and zero of left biceps
Hubertus 4:5ceb8f058874 34 double Rvector[200]; // Vector for Lwaarde configuration
Hubertus 4:5ceb8f058874 35 double Rwaarde[2]; // Vector for storage of max and zero of right biceps
Hubertus 4:5ceb8f058874 36 double Svector[200]; // Vector for Swaarde configuration
Hubertus 4:5ceb8f058874 37 double Swaarde[2]; // Vector for storage of max and zero of servo emg
Hubertus 0:df553b18547d 38 int x = 0; // Variable for switching between zero and max
Hubertus 4:5ceb8f058874 39 double movagR; // Moving Average mean value of right biceps
Hubertus 4:5ceb8f058874 40 double movagL; // Moving Average mean value of left biceps
Hubertus 4:5ceb8f058874 41 double movagS; // Moving Average mean value of servo spier
Hubertus 0:df553b18547d 42 float thresholdL = 10; // Startvalue for threshold, to block signals -
Hubertus 0:df553b18547d 43 float thresholdR = 10; // - before the zero and max values are calculated
Hubertus 4:5ceb8f058874 44 float thresholdS = 10; //-------
Hubertus 0:df553b18547d 45
Hubertus 4:5ceb8f058874 46 //------------Filter parameters----------------------
Hubertus 4:5ceb8f058874 47
Hubertus 4:5ceb8f058874 48 //Lowpassfilter
Hubertus 4:5ceb8f058874 49 //const double b0LP = 0.0014831498359569692;
Hubertus 4:5ceb8f058874 50 //const double b1LP = 0.0029662996719139385;
Hubertus 4:5ceb8f058874 51 //const double b2LP = 0.0014831498359569692;
Hubertus 4:5ceb8f058874 52 //const double a1LP = -1.918570032544273;
Hubertus 4:5ceb8f058874 53 //const double a2LP = 0.9245026318881009;
Hubertus 4:5ceb8f058874 54 //Highpassfilter Fc = 10 Hz;, Q = 0.5, Fs = 500 Hz
Hubertus 4:5ceb8f058874 55 const double b0HP = 0.8851221317817073;
Hubertus 4:5ceb8f058874 56 const double b1HP = -1.7702442635634146;
Hubertus 4:5ceb8f058874 57 const double b2HP = 0.8851221317817073;
Hubertus 4:5ceb8f058874 58 const double a1HP = -1.7632371847263784;
Hubertus 4:5ceb8f058874 59 const double a2HP = 0.777251342400451;
Hubertus 4:5ceb8f058874 60 //Notchfilter Fc = 50 Hz, Q = 10, Fs = 500 Hz
Hubertus 4:5ceb8f058874 61 const double b0NO = 0.9714498065192796;
Hubertus 4:5ceb8f058874 62 const double b1NO = -1.5718388053127037;
Hubertus 4:5ceb8f058874 63 const double b2NO = 0.9714498065192796;
Hubertus 4:5ceb8f058874 64 const double a1NO = -1.5718388053127037;
Hubertus 4:5ceb8f058874 65 const double a2NO = 0.9428996130385592;
Hubertus 4:5ceb8f058874 66
Hubertus 4:5ceb8f058874 67 //--------------Filter------------
Hubertus 4:5ceb8f058874 68 //BiQuad LPR( b0LP, b1LP, b2LP, a1LP, a2LP ); //Lowpass filter Biquad
Hubertus 4:5ceb8f058874 69 BiQuad HPR( b0HP, b1HP, b2HP, a1HP, a2HP ); //Highpass filter Biquad
Hubertus 4:5ceb8f058874 70 BiQuad NOR( b0NO, b1NO, b2NO, a1NO, a2NO ); //Notch filter Biquad
Hubertus 4:5ceb8f058874 71
Hubertus 4:5ceb8f058874 72 //BiQuad LPL( b0LP, b1LP, b2LP, a1LP, a2LP ); //Lowpass filter Biquad
Hubertus 4:5ceb8f058874 73 BiQuad HPL( b0HP, b1HP, b2HP, a1HP, a2HP ); //Highpass filter Biquad
Hubertus 4:5ceb8f058874 74 BiQuad NOL( b0NO, b1NO, b2NO, a1NO, a2NO ); //Notch filter Biquad
Hubertus 4:5ceb8f058874 75
Hubertus 4:5ceb8f058874 76 //BiQuad LPS( b0LP, b1LP, b2LP, a1LP, a2LP ); //Lowpass filter Biquad
Hubertus 4:5ceb8f058874 77 BiQuad HPS( b0HP, b1HP, b2HP, a1HP, a2HP ); //Highpass filter Biquad
Hubertus 4:5ceb8f058874 78 BiQuad NOS( b0NO, b1NO, b2NO, a1NO, a2NO ); //Notch filter Biquad
Hubertus 0:df553b18547d 79
Hubertus 0:df553b18547d 80
Hubertus 0:df553b18547d 81 void emgSample() { // EMG function
Hubertus 4:5ceb8f058874 82
Hubertus 4:5ceb8f058874 83 double emgNOFilteredL = NOL.step(emgL.read()); // Filtered NO value of EMG signal left biceps
Hubertus 4:5ceb8f058874 84 double emgHPFilteredL = HPL.step(emgNOFilteredL); // Filtered HP value of EMG signal left biceps
Hubertus 4:5ceb8f058874 85 double emgabsL = fabs(emgHPFilteredL); // Absolute value of EMG signal left biceps
Hubertus 4:5ceb8f058874 86
Hubertus 3:be5ac89a0b53 87
Hubertus 4:5ceb8f058874 88 double emgNOFilteredR = NOR.step(emgR.read()); // Filtered NO value of EMG signal right biceps
Hubertus 4:5ceb8f058874 89 double emgHPFilteredR = HPR.step(emgNOFilteredR); // Filtered HP value of EMG signal right biceps
Hubertus 4:5ceb8f058874 90 double emgabsR = fabs(emgHPFilteredR); // Absolute value of EMG signal right biceps
Hubertus 1:5c3259ecf10a 91
Hubertus 4:5ceb8f058874 92
Hubertus 4:5ceb8f058874 93 double emgNOFilteredS = NOS.step(emgS.read()); // Filtered NO value of EMG signal servo spier
Hubertus 4:5ceb8f058874 94 double emgHPFilteredS = HPS.step(emgNOFilteredS); // Filtered HP value of EMG signal servo spier
Hubertus 4:5ceb8f058874 95 double emgabsS = fabs(emgHPFilteredS); // Absolute value of EMG signal servo spier
Hubertus 2:36ad60c0aa01 96
Hubertus 1:5c3259ecf10a 97
Hubertus 0:df553b18547d 98
Hubertus 0:df553b18547d 99
Hubertus 4:5ceb8f058874 100 //-------------------Linker bicep-------------------------------------
Hubertus 4:5ceb8f058874 101
Hubertus 0:df553b18547d 102 for(int i = P-1; i >= 0; i--){ // For-loop used for moving average
Hubertus 0:df553b18547d 103 if (i == 0) {
Hubertus 2:36ad60c0aa01 104 A[i] = emgabsL;
Hubertus 0:df553b18547d 105 }
Hubertus 0:df553b18547d 106 else {
Hubertus 0:df553b18547d 107 A[i] = A[i-1];
Hubertus 0:df553b18547d 108 }
Hubertus 0:df553b18547d 109 }
Hubertus 0:df553b18547d 110 double sumL = 0;
Hubertus 0:df553b18547d 111 for (int n = 0; n < P-1; n++) { // Summation of array
Hubertus 0:df553b18547d 112 sumL = sumL + A[n];
Hubertus 0:df553b18547d 113 }
Hubertus 4:5ceb8f058874 114 movagL = sumL/P;
Hubertus 4:5ceb8f058874 115
Hubertus 0:df553b18547d 116
Hubertus 0:df553b18547d 117
Hubertus 4:5ceb8f058874 118 //--------------Rechter bicep---------------------------------------
Hubertus 4:5ceb8f058874 119
Hubertus 4:5ceb8f058874 120 for(int i = Q-1; i >= 0; i--){ // For-loop used for moving average
Hubertus 0:df553b18547d 121 if (i == 0) {
Hubertus 0:df553b18547d 122 B[i] = emgabsR;
Hubertus 0:df553b18547d 123 }
Hubertus 0:df553b18547d 124 else {
Hubertus 0:df553b18547d 125 B[i] = B[i-1];
Hubertus 0:df553b18547d 126 }
Hubertus 0:df553b18547d 127 }
Hubertus 0:df553b18547d 128 double sumR = 0;
Hubertus 0:df553b18547d 129
Hubertus 4:5ceb8f058874 130 for (int n = 0; n < Q-1; n++) { // Summation of array
Hubertus 0:df553b18547d 131 sumR = sumR + B[n];
Hubertus 0:df553b18547d 132 }
Hubertus 4:5ceb8f058874 133 movagR = sumR/Q; // Moving average value right biceps
Hubertus 4:5ceb8f058874 134
Hubertus 4:5ceb8f058874 135 //---------------Servo spier ------------------------------------
Hubertus 4:5ceb8f058874 136
Hubertus 4:5ceb8f058874 137 for(int i = R-1; i >= 0; i--){ // For-loop used for moving average
Hubertus 4:5ceb8f058874 138 if (i == 0) {
Hubertus 4:5ceb8f058874 139 C[i] = emgabsS;
Hubertus 4:5ceb8f058874 140 }
Hubertus 4:5ceb8f058874 141 else {
Hubertus 4:5ceb8f058874 142 C[i] = C[i-1];
Hubertus 4:5ceb8f058874 143 }
Hubertus 4:5ceb8f058874 144 }
Hubertus 4:5ceb8f058874 145 double sumS = 0;
Hubertus 4:5ceb8f058874 146 for (int n = 0; n < R-1; n++) { // Summation of array
Hubertus 4:5ceb8f058874 147 sumS = sumS + C[n];
Hubertus 4:5ceb8f058874 148 }
Hubertus 4:5ceb8f058874 149 movagS = sumS/R;
Hubertus 3:be5ac89a0b53 150
Hubertus 3:be5ac89a0b53 151 /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
Hubertus 4:5ceb8f058874 152 scope.set(0, emgL.read());
Hubertus 4:5ceb8f058874 153 scope.set(1, movagL);
Hubertus 4:5ceb8f058874 154 scope.set(2, emgR.read());
Hubertus 4:5ceb8f058874 155 scope.set(3, movagR);
Hubertus 4:5ceb8f058874 156 scope.set(4, emgS.read());
Hubertus 4:5ceb8f058874 157 scope.set(5, movagS);
Hubertus 3:be5ac89a0b53 158 /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels)
Hubertus 3:be5ac89a0b53 159 * Ensure that enough channels are available (HIDScope scope( 2 ))
Hubertus 3:be5ac89a0b53 160 * Finally, send all channels to the PC at once */
Hubertus 4:5ceb8f058874 161 scope.send(); // Moving average value left biceps
Hubertus 0:df553b18547d 162
Hubertus 0:df553b18547d 163
Hubertus 4:5ceb8f058874 164 if (CalButton==0 & k<200) { // Loop used for gaining max and zero value
Hubertus 4:5ceb8f058874 165 Lvector[k] = movagL;
Hubertus 4:5ceb8f058874 166 Rvector[k] = movagR;
Hubertus 4:5ceb8f058874 167 Svector[k] = movagS;
Hubertus 3:be5ac89a0b53 168
Hubertus 1:5c3259ecf10a 169 if (x==0){
Hubertus 1:5c3259ecf10a 170 ledB = !ledB; // SPIER NIET AANSPANNEN BIJ BLAUW
Hubertus 1:5c3259ecf10a 171 } // SPIER WEL AANSPANNEN BIJ ROOD
Hubertus 1:5c3259ecf10a 172 else if (x==1){
Hubertus 1:5c3259ecf10a 173 ledR = !ledR;
Hubertus 1:5c3259ecf10a 174 }
Hubertus 0:df553b18547d 175 k++;
Hubertus 0:df553b18547d 176 }
Hubertus 0:df553b18547d 177 else if (k==200) { // End of the loop, used for calculation value
Hubertus 0:df553b18547d 178 double sumY = 0;
Hubertus 0:df553b18547d 179 double sumZ = 0;
Hubertus 4:5ceb8f058874 180 double sumX = 0;
Hubertus 0:df553b18547d 181 for (int n = 0; n < 199; n++) {
Hubertus 0:df553b18547d 182 sumY = sumY + Lvector[n];
Hubertus 0:df553b18547d 183 sumZ = sumZ + Rvector[n];
Hubertus 4:5ceb8f058874 184 sumX = sumX + Svector[n];
Hubertus 0:df553b18547d 185 }
Hubertus 0:df553b18547d 186 Lwaarde[x] = sumY/200; // Value of zero for left biceps
Hubertus 4:5ceb8f058874 187 Rwaarde[x] = sumZ/200; // Value of zero for right biceps
Hubertus 4:5ceb8f058874 188 Swaarde[x] = sumX/200; // Value of zero for Servo spier
Hubertus 0:df553b18547d 189 k++;
Hubertus 0:df553b18547d 190 ledB = 1;
Hubertus 1:5c3259ecf10a 191 ledR = 1;
Hubertus 0:df553b18547d 192 ledG = !ledG;
Hubertus 0:df553b18547d 193 }
Hubertus 3:be5ac89a0b53 194 else if (k == 201 && zeromax ==0) { // Loop used for switching between zero and max
Hubertus 0:df553b18547d 195 ledG = !ledG;
Hubertus 0:df553b18547d 196 k = 0;
Hubertus 0:df553b18547d 197 if (x==0) {
Hubertus 0:df553b18547d 198 x++;
Hubertus 0:df553b18547d 199 }
Hubertus 0:df553b18547d 200 else if (x==1) {
Hubertus 0:df553b18547d 201 x=0;
Hubertus 0:df553b18547d 202 }
Hubertus 0:df553b18547d 203 }
Hubertus 0:df553b18547d 204 if (x==1) // Determining threshold using zero and max
Hubertus 0:df553b18547d 205 {
Hubertus 0:df553b18547d 206 thresholdL = Lwaarde[0]+(Lwaarde[1]-Lwaarde[0])*(0.25f);
Hubertus 0:df553b18547d 207 thresholdR = Rwaarde[0]+(Rwaarde[1]-Rwaarde[0])*(0.25f);
Hubertus 4:5ceb8f058874 208 thresholdS = Swaarde[0]+(Swaarde[1]-Swaarde[0])*(0.25f);
Hubertus 0:df553b18547d 209 }
Hubertus 0:df553b18547d 210 }
Hubertus 0:df553b18547d 211
Hubertus 0:df553b18547d 212 int main()
Hubertus 0:df553b18547d 213 {
Hubertus 0:df553b18547d 214 ledB = 1;
Hubertus 0:df553b18547d 215 ledG = 1;
Hubertus 1:5c3259ecf10a 216 ledR = 1;
Hubertus 0:df553b18547d 217 emgSampleTicker.attach( &emgSample, 0.002 ); // Ticker for EMG function
Hubertus 0:df553b18547d 218 Lwaarde[0] = 0.01; // Startvalue for zerovalue, to -
Hubertus 0:df553b18547d 219 Rwaarde[0] = 0.01; // - prevent dividing by 0
Hubertus 4:5ceb8f058874 220 Swaarde[0] = 0.01;
Hubertus 0:df553b18547d 221 while(1) {
Hubertus 3:be5ac89a0b53 222
Hubertus 4:5ceb8f058874 223 // if (movmeanR > thresholdR)
Hubertus 4:5ceb8f058874 224 // { myservo = 0.5;
Hubertus 4:5ceb8f058874 225 // wait(0.01);
Hubertus 4:5ceb8f058874 226 // }
Hubertus 4:5ceb8f058874 227 // else {
Hubertus 4:5ceb8f058874 228 // myservo = 0.0;
Hubertus 4:5ceb8f058874 229 // wait(0.01);
Hubertus 4:5ceb8f058874 230 // }
Hubertus 4:5ceb8f058874 231 //
Hubertus 4:5ceb8f058874 232 // if (movmeanL > thresholdL)
Hubertus 4:5ceb8f058874 233 // { motor1control.write(0.8);
Hubertus 4:5ceb8f058874 234 // motor1direction.write(true);
Hubertus 4:5ceb8f058874 235 // }
Hubertus 4:5ceb8f058874 236 // else {
Hubertus 4:5ceb8f058874 237 // motor1control.write(0);
Hubertus 4:5ceb8f058874 238 // }
Hubertus 0:df553b18547d 239 }
Hubertus 0:df553b18547d 240 }