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@3:be5ac89a0b53, 2018-10-19 (annotated)

Committer:
Hubertus
Date:
Fri Oct 19 08:05:58 2018 +0000
Revision:
3:be5ac89a0b53
Parent:
2:36ad60c0aa01
Child:
4:5ceb8f058874
Servo werkt samen met 1 DC motor, beide aangestuurd met EMG

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 3:be5ac89a0b53 4 #include "Servo.h"
Hubertus 3:be5ac89a0b53 5 //#include "HIDScope.h"
Hubertus 0:df553b18547d 6
Hubertus 1:5c3259ecf10a 7
Hubertus 3:be5ac89a0b53 8
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 3:be5ac89a0b53 16 DigitalIn zeromax(PTC6); // Button used for switching between zero and max
Hubertus 0:df553b18547d 17 Ticker emgSampleTicker; // Ticker for sample frequency
Hubertus 3:be5ac89a0b53 18 //HIDScope scope( 4 );
Hubertus 3:be5ac89a0b53 19 Servo myservo(D13);
Hubertus 3:be5ac89a0b53 20 DigitalOut motor1direction(D7);
Hubertus 3:be5ac89a0b53 21 PwmOut motor1control(D6);
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 3:be5ac89a0b53 57
Hubertus 3:be5ac89a0b53 58 double emgNOFilteredR = bq3R.step(emgR.read()); // Filtered HP value of EMG signal left biceps
Hubertus 3:be5ac89a0b53 59 double emgHPFilteredR = bq2R.step(emgNOFilteredR); // Filtered HP value of EMG signal left biceps
Hubertus 3:be5ac89a0b53 60 double emgabsR = fabs(emgHPFilteredR); // Absolute value of EMG signal left biceps
Hubertus 1:5c3259ecf10a 61
Hubertus 1:5c3259ecf10a 62 double emgNOFilteredL = bq3L.step(emgL.read()); // Filtered HP value of EMG signal left biceps
Hubertus 1:5c3259ecf10a 63 double emgHPFilteredL = bq2L.step(emgNOFilteredL); // Filtered HP value of EMG signal left biceps
Hubertus 2:36ad60c0aa01 64 double emgabsL = fabs(emgHPFilteredL); // Absolute value of EMG signal left biceps
Hubertus 2:36ad60c0aa01 65
Hubertus 1:5c3259ecf10a 66
Hubertus 0:df553b18547d 67
Hubertus 0:df553b18547d 68
Hubertus 0:df553b18547d 69 for(int i = P-1; i >= 0; i--){ // For-loop used for moving average
Hubertus 0:df553b18547d 70 if (i == 0) {
Hubertus 2:36ad60c0aa01 71 A[i] = emgabsL;
Hubertus 0:df553b18547d 72 }
Hubertus 0:df553b18547d 73 else {
Hubertus 0:df553b18547d 74 A[i] = A[i-1];
Hubertus 0:df553b18547d 75 }
Hubertus 0:df553b18547d 76 }
Hubertus 0:df553b18547d 77 double sumL = 0;
Hubertus 0:df553b18547d 78 for (int n = 0; n < P-1; n++) { // Summation of array
Hubertus 0:df553b18547d 79 sumL = sumL + A[n];
Hubertus 0:df553b18547d 80 }
Hubertus 0:df553b18547d 81 movmeanL = sumL/P;
Hubertus 0:df553b18547d 82
Hubertus 0:df553b18547d 83
Hubertus 0:df553b18547d 84 for(int i = P-1; i >= 0; i--){ // For-loop used for moving average
Hubertus 0:df553b18547d 85 if (i == 0) {
Hubertus 0:df553b18547d 86 B[i] = emgabsR;
Hubertus 0:df553b18547d 87 }
Hubertus 0:df553b18547d 88 else {
Hubertus 0:df553b18547d 89 B[i] = B[i-1];
Hubertus 0:df553b18547d 90 }
Hubertus 0:df553b18547d 91 }
Hubertus 0:df553b18547d 92 double sumR = 0;
Hubertus 0:df553b18547d 93
Hubertus 0:df553b18547d 94 for (int n = 0; n < P-1; n++) { // Summation of array
Hubertus 0:df553b18547d 95 sumR = sumR + B[n];
Hubertus 0:df553b18547d 96 }
Hubertus 0:df553b18547d 97 movmeanR = sumR/P; //dit is de moving average waarde // Moving average value right biceps
Hubertus 3:be5ac89a0b53 98
Hubertus 3:be5ac89a0b53 99 /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
Hubertus 3:be5ac89a0b53 100 // scope.set(0, emgL.read() );
Hubertus 3:be5ac89a0b53 101 //scope.set(1, movmeanL);
Hubertus 3:be5ac89a0b53 102 //scope.set(2, emgR.read());
Hubertus 3:be5ac89a0b53 103 //scope.set(3, movmeanR);
Hubertus 3:be5ac89a0b53 104 /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels)
Hubertus 3:be5ac89a0b53 105 * Ensure that enough channels are available (HIDScope scope( 2 ))
Hubertus 3:be5ac89a0b53 106 * Finally, send all channels to the PC at once */
Hubertus 3:be5ac89a0b53 107 // scope.send(); // Moving average value left biceps
Hubertus 0:df553b18547d 108
Hubertus 0:df553b18547d 109
Hubertus 0:df553b18547d 110 if (TestButton==0 & k<200) { // Loop used for gaining max and zero value
Hubertus 0:df553b18547d 111 Lvector[k] = movmeanL;
Hubertus 0:df553b18547d 112 Rvector[k] = movmeanR;
Hubertus 3:be5ac89a0b53 113
Hubertus 1:5c3259ecf10a 114 if (x==0){
Hubertus 1:5c3259ecf10a 115 ledB = !ledB; // SPIER NIET AANSPANNEN BIJ BLAUW
Hubertus 1:5c3259ecf10a 116 } // SPIER WEL AANSPANNEN BIJ ROOD
Hubertus 1:5c3259ecf10a 117 else if (x==1){
Hubertus 1:5c3259ecf10a 118 ledR = !ledR;
Hubertus 1:5c3259ecf10a 119 }
Hubertus 0:df553b18547d 120 k++;
Hubertus 0:df553b18547d 121 }
Hubertus 0:df553b18547d 122 else if (k==200) { // End of the loop, used for calculation value
Hubertus 0:df553b18547d 123 double sumY = 0;
Hubertus 0:df553b18547d 124 double sumZ = 0;
Hubertus 0:df553b18547d 125 for (int n = 0; n < 199; n++) {
Hubertus 0:df553b18547d 126 sumY = sumY + Lvector[n];
Hubertus 0:df553b18547d 127 sumZ = sumZ + Rvector[n];
Hubertus 0:df553b18547d 128 }
Hubertus 0:df553b18547d 129 Lwaarde[x] = sumY/200; // Value of zero for left biceps
Hubertus 0:df553b18547d 130 Rwaarde[x] = sumZ/200; // Value of zero for rigth biceps
Hubertus 0:df553b18547d 131 k++;
Hubertus 0:df553b18547d 132 ledB = 1;
Hubertus 1:5c3259ecf10a 133 ledR = 1;
Hubertus 0:df553b18547d 134 ledG = !ledG;
Hubertus 0:df553b18547d 135 }
Hubertus 3:be5ac89a0b53 136 else if (k == 201 && zeromax ==0) { // Loop used for switching between zero and max
Hubertus 0:df553b18547d 137 ledG = !ledG;
Hubertus 0:df553b18547d 138 k = 0;
Hubertus 0:df553b18547d 139 if (x==0) {
Hubertus 0:df553b18547d 140 x++;
Hubertus 0:df553b18547d 141 }
Hubertus 0:df553b18547d 142 else if (x==1) {
Hubertus 0:df553b18547d 143 x=0;
Hubertus 0:df553b18547d 144 }
Hubertus 0:df553b18547d 145 }
Hubertus 0:df553b18547d 146 if (x==1) // Determining threshold using zero and max
Hubertus 0:df553b18547d 147 {
Hubertus 0:df553b18547d 148 thresholdL = Lwaarde[0]+(Lwaarde[1]-Lwaarde[0])*(0.25f);
Hubertus 0:df553b18547d 149 thresholdR = Rwaarde[0]+(Rwaarde[1]-Rwaarde[0])*(0.25f);
Hubertus 0:df553b18547d 150 }
Hubertus 0:df553b18547d 151 }
Hubertus 0:df553b18547d 152
Hubertus 0:df553b18547d 153 int main()
Hubertus 0:df553b18547d 154 {
Hubertus 0:df553b18547d 155 ledB = 1;
Hubertus 0:df553b18547d 156 ledG = 1;
Hubertus 1:5c3259ecf10a 157 ledR = 1;
Hubertus 0:df553b18547d 158 emgSampleTicker.attach( &emgSample, 0.002 ); // Ticker for EMG function
Hubertus 0:df553b18547d 159 Lwaarde[0] = 0.01; // Startvalue for zerovalue, to -
Hubertus 0:df553b18547d 160 Rwaarde[0] = 0.01; // - prevent dividing by 0
Hubertus 0:df553b18547d 161 while(1) {
Hubertus 3:be5ac89a0b53 162
Hubertus 3:be5ac89a0b53 163 if (movmeanR > thresholdR)
Hubertus 3:be5ac89a0b53 164 { myservo = 0.5;
Hubertus 3:be5ac89a0b53 165 wait(0.01);
Hubertus 3:be5ac89a0b53 166 }
Hubertus 3:be5ac89a0b53 167 else {
Hubertus 3:be5ac89a0b53 168 myservo = 0.0;
Hubertus 3:be5ac89a0b53 169 wait(0.01);
Hubertus 3:be5ac89a0b53 170 }
Hubertus 3:be5ac89a0b53 171
Hubertus 3:be5ac89a0b53 172 if (movmeanL > thresholdL)
Hubertus 3:be5ac89a0b53 173 { motor1control.write(0.8);
Hubertus 3:be5ac89a0b53 174 motor1direction.write(true);
Hubertus 3:be5ac89a0b53 175 }
Hubertus 3:be5ac89a0b53 176 else {
Hubertus 3:be5ac89a0b53 177 motor1control.write(0);
Hubertus 3:be5ac89a0b53 178 }
Hubertus 0:df553b18547d 179 }
Hubertus 0:df553b18547d 180 }