Tim Luten / 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_TEST3 by Biorobotica TIC

main.cpp@6:1f722bf6a89b, 2018-10-26 (annotated)

Committer:
Hubertus
Date:
Fri Oct 26 08:53:26 2018 +0000
Revision:
6:1f722bf6a89b
Parent:
4:5ceb8f058874
Child:
7:b53f0c4cf2b9
bezig met alles samen

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