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@0:df553b18547d, 2018-10-17 (annotated)

Committer:
Hubertus
Date:
Wed Oct 17 11:19:42 2018 +0000
Revision:
0:df553b18547d
Child:
1:5c3259ecf10a
Zoals jan het heeft gedaan

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