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@2:36ad60c0aa01, 2018-10-18 (annotated)

Committer:
Hubertus
Date:
Thu Oct 18 14:16:08 2018 +0000
Revision:
2:36ad60c0aa01
Parent:
1:5c3259ecf10a
Child:
3:be5ac89a0b53
Alles voor elkaar (NOTCH HP FABS MOVAG)

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 1:5c3259ecf10a 4 //#include "Servo.h"
Hubertus 0:df553b18547d 5 #include "HIDScope.h"
Hubertus 0:df553b18547d 6
Hubertus 1:5c3259ecf10a 7
Hubertus 1:5c3259ecf10a 8 //Servo myservo(D7);
Hubertus 0:df553b18547d 9 //Define objects
Hubertus 0:df553b18547d 10 AnalogIn emgL( A0 ); // EMG Left Arm
Hubertus 0:df553b18547d 11 AnalogIn emgR( A1 ); // EMG Right Arm
Hubertus 0:df553b18547d 12 DigitalOut ledB(LED_BLUE); // Informative LED for gaining zero and max
Hubertus 1:5c3259ecf10a 13 DigitalOut ledR(LED_RED);
Hubertus 0:df553b18547d 14 DigitalOut ledG(LED_GREEN); // Informative LED for gaining zero and max
Hubertus 0:df553b18547d 15 DigitalIn TestButton(PTA4); // Button used for gaining zero and max
Hubertus 0:df553b18547d 16 DigitalIn onoff(PTC6); // Button used for switching between zero and max
Hubertus 0:df553b18547d 17 Ticker emgSampleTicker; // Ticker for sample frequency
Hubertus 0:df553b18547d 18 DigitalOut signalL(D4); // Output to motorshield, emg left biceps
Hubertus 0:df553b18547d 19 DigitalOut signalR(D5);
Hubertus 0:df553b18547d 20 HIDScope scope( 2 ); // Output to motorshield, emg right biceps
Hubertus 0:df553b18547d 21
Hubertus 0:df553b18547d 22
Hubertus 0:df553b18547d 23
Hubertus 0:df553b18547d 24 int P= 200; // Number of points for moving average first emg
Hubertus 0:df553b18547d 25 int Q = 200; // Number of points for moving average second emg
Hubertus 0:df553b18547d 26 double A[200]; // Vector for storing data of first emg
Hubertus 0:df553b18547d 27 double B[200]; // Vector for storing data of second emg
Hubertus 0:df553b18547d 28 int k = 0; // Counter for configuration:
Hubertus 0:df553b18547d 29 double Rvector[200]; // Vector for Rwaarde configuration
Hubertus 0:df553b18547d 30 double Rwaarde[2]; // Vector for storage of max and zero of left biceps
Hubertus 0:df553b18547d 31 double Lvector[200]; // Vector for Lwaarde configuration
Hubertus 0:df553b18547d 32 double Lwaarde[2]; // Vector for storage of max and zero of right biceps
Hubertus 0:df553b18547d 33 int x = 0; // Variable for switching between zero and max
Hubertus 0:df553b18547d 34 double movmeanR; // Moving Average mean value of right biceps
Hubertus 0:df553b18547d 35 double movmeanL; // Moving Average mean value of left biceps
Hubertus 0:df553b18547d 36 float thresholdL = 10; // Startvalue for threshold, to block signals -
Hubertus 0:df553b18547d 37 float thresholdR = 10; // - before the zero and max values are calculated
Hubertus 0:df553b18547d 38
Hubertus 0:df553b18547d 39 // Filters
Hubertus 0:df553b18547d 40 BiQuadChain bqcR; // BiQuad for signal right biceps
Hubertus 0:df553b18547d 41 BiQuad bq1R( 0.6844323315947305,1.368864663189461, 0.6844323315947305,
Hubertus 0:df553b18547d 42 1.2243497755555954,0.5133795508233265); // LP
Hubertus 0:df553b18547d 43 BiQuad bq2R( 0.6844323315947306, -1.3688646631894612, 0.6844323315947306,
Hubertus 0:df553b18547d 44 -1.2243497755555959, 0.5133795508233266); // HP
Hubertus 0:df553b18547d 45 BiQuad bq3R( 0.7566897754116633, -1.2243497755555959, 0.7566897754116633,
Hubertus 0:df553b18547d 46 -1.2243497755555959, 0.5133795508233266); // Notch
Hubertus 0:df553b18547d 47 BiQuadChain bqcL; // BiQuad for signal left biceps
Hubertus 0:df553b18547d 48 BiQuad bq1L( 0.6844323315947305,1.368864663189461, 0.6844323315947305,
Hubertus 0:df553b18547d 49 1.2243497755555954,0.5133795508233265); // LP
Hubertus 0:df553b18547d 50 BiQuad bq2L( 0.6844323315947306, -1.3688646631894612, 0.6844323315947306,
Hubertus 0:df553b18547d 51 -1.2243497755555959, 0.5133795508233266); // HP
Hubertus 0:df553b18547d 52 BiQuad bq3L( 0.7566897754116633, -1.2243497755555959, 0.7566897754116633,
Hubertus 0:df553b18547d 53 -1.2243497755555959, 0.5133795508233266); // Notch
Hubertus 0:df553b18547d 54
Hubertus 0:df553b18547d 55
Hubertus 0:df553b18547d 56 void emgSample() { // EMG function
Hubertus 0:df553b18547d 57
Hubertus 1:5c3259ecf10a 58 double emgFilteredR = bqcR.step( emgR.read() );
Hubertus 1:5c3259ecf10a 59 double emgabsR = abs(emgFilteredR); // Absolute value of EMG signal right biceps
Hubertus 1:5c3259ecf10a 60
Hubertus 1:5c3259ecf10a 61 double emgNOFilteredL = bq3L.step(emgL.read()); // Filtered HP value of EMG signal left biceps
Hubertus 1:5c3259ecf10a 62 double emgHPFilteredL = bq2L.step(emgNOFilteredL); // Filtered HP value of EMG signal left biceps
Hubertus 2:36ad60c0aa01 63 double emgabsL = fabs(emgHPFilteredL); // Absolute value of EMG signal left biceps
Hubertus 2:36ad60c0aa01 64
Hubertus 1:5c3259ecf10a 65
Hubertus 0:df553b18547d 66
Hubertus 0:df553b18547d 67
Hubertus 0:df553b18547d 68 for(int i = P-1; i >= 0; i--){ // For-loop used for moving average
Hubertus 0:df553b18547d 69 if (i == 0) {
Hubertus 2:36ad60c0aa01 70 A[i] = emgabsL;
Hubertus 0:df553b18547d 71 }
Hubertus 0:df553b18547d 72 else {
Hubertus 0:df553b18547d 73 A[i] = A[i-1];
Hubertus 0:df553b18547d 74 }
Hubertus 0:df553b18547d 75 }
Hubertus 0:df553b18547d 76 double sumL = 0;
Hubertus 0:df553b18547d 77 for (int n = 0; n < P-1; n++) { // Summation of array
Hubertus 0:df553b18547d 78 sumL = sumL + A[n];
Hubertus 0:df553b18547d 79 }
Hubertus 0:df553b18547d 80 movmeanL = sumL/P;
Hubertus 0:df553b18547d 81
Hubertus 0:df553b18547d 82 /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
Hubertus 0:df553b18547d 83 scope.set(0, emgL.read() );
Hubertus 0:df553b18547d 84 scope.set(1, movmeanL);
Hubertus 0:df553b18547d 85 /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels)
Hubertus 0:df553b18547d 86 * Ensure that enough channels are available (HIDScope scope( 2 ))
Hubertus 0:df553b18547d 87 * Finally, send all channels to the PC at once */
Hubertus 0:df553b18547d 88 scope.send(); // Moving average value left biceps
Hubertus 0:df553b18547d 89
Hubertus 0:df553b18547d 90 for(int i = P-1; i >= 0; i--){ // For-loop used for moving average
Hubertus 0:df553b18547d 91 if (i == 0) {
Hubertus 0:df553b18547d 92 B[i] = emgabsR;
Hubertus 0:df553b18547d 93 }
Hubertus 0:df553b18547d 94 else {
Hubertus 0:df553b18547d 95 B[i] = B[i-1];
Hubertus 0:df553b18547d 96 }
Hubertus 0:df553b18547d 97 }
Hubertus 0:df553b18547d 98 double sumR = 0;
Hubertus 0:df553b18547d 99
Hubertus 0:df553b18547d 100 for (int n = 0; n < P-1; n++) { // Summation of array
Hubertus 0:df553b18547d 101 sumR = sumR + B[n];
Hubertus 0:df553b18547d 102 }
Hubertus 0:df553b18547d 103 movmeanR = sumR/P; //dit is de moving average waarde // Moving average value right biceps
Hubertus 0:df553b18547d 104
Hubertus 0:df553b18547d 105
Hubertus 0:df553b18547d 106 if (TestButton==0 & k<200) { // Loop used for gaining max and zero value
Hubertus 0:df553b18547d 107 Lvector[k] = movmeanL;
Hubertus 0:df553b18547d 108 Rvector[k] = movmeanR;
Hubertus 1:5c3259ecf10a 109 if (x==0){
Hubertus 1:5c3259ecf10a 110 ledB = !ledB; // SPIER NIET AANSPANNEN BIJ BLAUW
Hubertus 1:5c3259ecf10a 111 } // SPIER WEL AANSPANNEN BIJ ROOD
Hubertus 1:5c3259ecf10a 112 else if (x==1){
Hubertus 1:5c3259ecf10a 113 ledR = !ledR;
Hubertus 1:5c3259ecf10a 114 }
Hubertus 0:df553b18547d 115 k++;
Hubertus 0:df553b18547d 116 }
Hubertus 0:df553b18547d 117 else if (k==200) { // End of the loop, used for calculation value
Hubertus 0:df553b18547d 118 double sumY = 0;
Hubertus 0:df553b18547d 119 double sumZ = 0;
Hubertus 0:df553b18547d 120 for (int n = 0; n < 199; n++) {
Hubertus 0:df553b18547d 121 sumY = sumY + Lvector[n];
Hubertus 0:df553b18547d 122 sumZ = sumZ + Rvector[n];
Hubertus 0:df553b18547d 123 }
Hubertus 0:df553b18547d 124 Lwaarde[x] = sumY/200; // Value of zero for left biceps
Hubertus 0:df553b18547d 125 Rwaarde[x] = sumZ/200; // Value of zero for rigth biceps
Hubertus 0:df553b18547d 126 k++;
Hubertus 0:df553b18547d 127 ledB = 1;
Hubertus 1:5c3259ecf10a 128 ledR = 1;
Hubertus 0:df553b18547d 129 ledG = !ledG;
Hubertus 0:df553b18547d 130 }
Hubertus 0:df553b18547d 131 else if (k == 201 && onoff ==0) { // Loop used for switching between zero and max
Hubertus 0:df553b18547d 132 ledG = !ledG;
Hubertus 0:df553b18547d 133 k = 0;
Hubertus 0:df553b18547d 134 if (x==0) {
Hubertus 0:df553b18547d 135 x++;
Hubertus 0:df553b18547d 136 }
Hubertus 0:df553b18547d 137 else if (x==1) {
Hubertus 0:df553b18547d 138 x=0;
Hubertus 0:df553b18547d 139 }
Hubertus 0:df553b18547d 140 }
Hubertus 0:df553b18547d 141 if (x==1) // Determining threshold using zero and max
Hubertus 0:df553b18547d 142 {
Hubertus 0:df553b18547d 143 thresholdL = Lwaarde[0]+(Lwaarde[1]-Lwaarde[0])*(0.25f);
Hubertus 0:df553b18547d 144 thresholdR = Rwaarde[0]+(Rwaarde[1]-Rwaarde[0])*(0.25f);
Hubertus 0:df553b18547d 145 }
Hubertus 0:df553b18547d 146
Hubertus 0:df553b18547d 147 if (movmeanL > thresholdL) // Signal sent to the motors
Hubertus 0:df553b18547d 148 {
Hubertus 0:df553b18547d 149 signalL = 1;
Hubertus 0:df553b18547d 150 }
Hubertus 0:df553b18547d 151 else
Hubertus 0:df553b18547d 152 {
Hubertus 0:df553b18547d 153 signalL = 0;
Hubertus 0:df553b18547d 154 }
Hubertus 0:df553b18547d 155
Hubertus 0:df553b18547d 156 if (movmeanR > thresholdR)
Hubertus 0:df553b18547d 157 {
Hubertus 0:df553b18547d 158 signalR = 1;
Hubertus 0:df553b18547d 159 }
Hubertus 0:df553b18547d 160 else
Hubertus 0:df553b18547d 161 {
Hubertus 0:df553b18547d 162 signalR = 0;
Hubertus 0:df553b18547d 163 }
Hubertus 0:df553b18547d 164 }
Hubertus 0:df553b18547d 165
Hubertus 0:df553b18547d 166 int main()
Hubertus 0:df553b18547d 167 {
Hubertus 0:df553b18547d 168 ledB = 1;
Hubertus 0:df553b18547d 169 ledG = 1;
Hubertus 1:5c3259ecf10a 170 ledR = 1;
Hubertus 0:df553b18547d 171 bqcR.add( &bq1R ).add( &bq2R ).add( &bq3R ); // Filter for emg signal
Hubertus 0:df553b18547d 172 emgSampleTicker.attach( &emgSample, 0.002 ); // Ticker for EMG function
Hubertus 0:df553b18547d 173 Lwaarde[0] = 0.01; // Startvalue for zerovalue, to -
Hubertus 0:df553b18547d 174 Rwaarde[0] = 0.01; // - prevent dividing by 0
Hubertus 0:df553b18547d 175 while(1) {
Hubertus 1:5c3259ecf10a 176 //
Hubertus 1:5c3259ecf10a 177 // if (movmeanL > (0.02))
Hubertus 1:5c3259ecf10a 178 // { myservo = 0.5;
Hubertus 1:5c3259ecf10a 179 // wait(0.01);
Hubertus 1:5c3259ecf10a 180 // }
Hubertus 1:5c3259ecf10a 181 // else {
Hubertus 1:5c3259ecf10a 182 // myservo = 0.0;
Hubertus 1:5c3259ecf10a 183 // wait(0.01);
Hubertus 1:5c3259ecf10a 184 // }
Hubertus 0:df553b18547d 185 }
Hubertus 0:df553b18547d 186 }