to send emg signal to motor with test programm
Dependencies: HIDScope biquadFilter mbed
Fork of EMGvoorjan by
main.cpp@28:4b22455930ff, 2017-10-25 (annotated)
- Committer:
- Roytsg
- Date:
- Wed Oct 25 11:24:25 2017 +0000
- Revision:
- 28:4b22455930ff
- Parent:
- 27:674193a62e06
- Child:
- 29:a48b63e60a40
emg-signal compleet;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
vsluiter | 0:32bb76391d89 | 1 | #include "mbed.h" |
vsluiter | 11:ce72ec658a95 | 2 | #include "HIDScope.h" |
Roytsg | 21:77998ce2c0dd | 3 | #include "BiQuad.h" |
Roytsg | 28:4b22455930ff | 4 | #include "math.h" |
Roytsg | 28:4b22455930ff | 5 | #include "Serial.h" |
vsluiter | 0:32bb76391d89 | 6 | |
Roytsg | 24:fe3825337233 | 7 | |
vsluiter | 4:8b298dfada81 | 8 | //Define objects |
Roytsg | 28:4b22455930ff | 9 | AnalogIn emg( A0 ); //EMG |
Roytsg | 28:4b22455930ff | 10 | AnalogIn emg1( A1 ); //EMG |
Roytsg | 28:4b22455930ff | 11 | HIDScope scope( 5 ); // aantal scopes dat gemaakt kan worden |
Roytsg | 28:4b22455930ff | 12 | DigitalOut ledB(LED_BLUE); |
Roytsg | 28:4b22455930ff | 13 | DigitalOut ledG(LED_GREEN); |
Roytsg | 28:4b22455930ff | 14 | DigitalIn TestButton(PTA4); // button naast het ledje |
Roytsg | 28:4b22455930ff | 15 | DigitalIn onoff(PTC6); // button aan de andere kant |
Roytsg | 28:4b22455930ff | 16 | Ticker emgSampleTicker; // Ticker voor de sample frequency |
vsluiter | 2:e314bb3b2d99 | 17 | |
Roytsg | 28:4b22455930ff | 18 | |
Roytsg | 28:4b22455930ff | 19 | int P= 200; // aantal test punten voor de moving average |
Roytsg | 28:4b22455930ff | 20 | double A[200]; // de vector waar punten in worden opgeslagen voor de moving average moet even groot zijn als P |
Roytsg | 28:4b22455930ff | 21 | int k = 0; // counter voor de configuratie |
Roytsg | 28:4b22455930ff | 22 | double Vvector[200]; // vector voor de Vwaarde configuratie |
Roytsg | 28:4b22455930ff | 23 | double Vwaarde[2]; // vector voor waardes van V |
Roytsg | 28:4b22455930ff | 24 | int x = 0; // x waarde voor de Vwaarde |
Roytsg | 24:fe3825337233 | 25 | |
Roytsg | 28:4b22455930ff | 26 | // Filters |
Roytsg | 28:4b22455930ff | 27 | BiQuadChain bqc; |
Roytsg | 28:4b22455930ff | 28 | BiQuad bq1( 0.6844323315947305,1.368864663189461, 0.6844323315947305,1.2243497755555954,0.5133795508233265); //lp? |
Roytsg | 28:4b22455930ff | 29 | BiQuad bq2( 0.6844323315947306, -1.3688646631894612, 0.6844323315947306, -1.2243497755555959, 0.5133795508233266); //hp? |
Roytsg | 28:4b22455930ff | 30 | BiQuad bq3( 0.7566897754116633, -1.2243497755555959, 0.7566897754116633, -1.2243497755555959, 0.5133795508233266); // notch? |
vsluiter | 0:32bb76391d89 | 31 | |
Roytsg | 28:4b22455930ff | 32 | |
Roytsg | 21:77998ce2c0dd | 33 | |
Roytsg | 28:4b22455930ff | 34 | // sample function voor plotten van de emg signalen en moving average |
Roytsg | 21:77998ce2c0dd | 35 | void emgSample() { |
Roytsg | 21:77998ce2c0dd | 36 | |
Roytsg | 28:4b22455930ff | 37 | double emgFiltered = bqc.step( emg.read() ); // gefilterde waarde van het emg signaal |
Roytsg | 28:4b22455930ff | 38 | double emgabs = abs(emgFiltered); // absolute waarde van het gefilterde signaal |
Roytsg | 28:4b22455930ff | 39 | scope.set(0, emgFiltered ); // stuurt de waarden naar de grafiek |
Roytsg | 28:4b22455930ff | 40 | scope.set(1, emgabs ); // stuurt de waarden naar de grafiek |
Roytsg | 24:fe3825337233 | 41 | |
Roytsg | 28:4b22455930ff | 42 | // deze for-loop maakt de vector voor de moving average |
Roytsg | 27:674193a62e06 | 43 | for(int i = P-1; i >= 0; i--){ |
Roytsg | 26:97a8adc9b895 | 44 | if (i == 0) { |
Roytsg | 26:97a8adc9b895 | 45 | A[i] = emgabs; |
Roytsg | 26:97a8adc9b895 | 46 | } |
Roytsg | 26:97a8adc9b895 | 47 | else { |
Roytsg | 26:97a8adc9b895 | 48 | A[i] = A[i-1]; |
Roytsg | 26:97a8adc9b895 | 49 | } |
Roytsg | 28:4b22455930ff | 50 | } |
Roytsg | 26:97a8adc9b895 | 51 | double sum = 0; |
Roytsg | 28:4b22455930ff | 52 | // deze for-loop sommeert de array |
Roytsg | 27:674193a62e06 | 53 | for (int n = 0; n < P-1; n++) { |
Roytsg | 26:97a8adc9b895 | 54 | sum = sum + A[n]; |
Roytsg | 26:97a8adc9b895 | 55 | } |
Roytsg | 24:fe3825337233 | 56 | |
Roytsg | 28:4b22455930ff | 57 | double movmean = sum/P; //dit is de moving average waarde |
Roytsg | 24:fe3825337233 | 58 | |
Roytsg | 28:4b22455930ff | 59 | // hier wordt het test programma opgestart, zodat zero waarde kan worden gekregen |
Roytsg | 28:4b22455930ff | 60 | if (TestButton==0 & k<200) { |
Roytsg | 28:4b22455930ff | 61 | Vvector[k] = movmean; |
Roytsg | 28:4b22455930ff | 62 | ledB = !ledB; |
Roytsg | 28:4b22455930ff | 63 | k++; |
Roytsg | 28:4b22455930ff | 64 | } |
Roytsg | 28:4b22455930ff | 65 | else if (k==200) { // hier moet de test klaar zijn |
Roytsg | 28:4b22455930ff | 66 | double sumZ = 0; |
Roytsg | 28:4b22455930ff | 67 | for (int n = 0; n < 199; n++) { |
Roytsg | 28:4b22455930ff | 68 | sumZ = sumZ + Vvector[n]; |
Roytsg | 28:4b22455930ff | 69 | } // neemt de som van de zerovector array |
Roytsg | 28:4b22455930ff | 70 | Vwaarde[x] = sumZ/200; // dit is het gemiddelde voor een betrouwbare value |
Roytsg | 28:4b22455930ff | 71 | scope.set(3,Vwaarde[0]); //stuurt de zeroV waarde naar het plotje |
Roytsg | 28:4b22455930ff | 72 | if (x == 1) { |
Roytsg | 28:4b22455930ff | 73 | scope.set(4,Vwaarde[1]); //stuurt de maxV waarde naar het plotje |
Roytsg | 28:4b22455930ff | 74 | } |
Roytsg | 28:4b22455930ff | 75 | k++; |
Roytsg | 28:4b22455930ff | 76 | ledB = 1; |
Roytsg | 28:4b22455930ff | 77 | ledG = !ledG; |
Roytsg | 28:4b22455930ff | 78 | } |
Roytsg | 28:4b22455930ff | 79 | else if (k == 201 && onoff ==0) {// dit is om het ledje uit te doen en om het mogelijk te maken de test opnieuw te doen |
Roytsg | 28:4b22455930ff | 80 | ledG = !ledG; |
Roytsg | 28:4b22455930ff | 81 | k = 0; |
Roytsg | 28:4b22455930ff | 82 | if (x==0) { |
Roytsg | 28:4b22455930ff | 83 | x++; |
Roytsg | 28:4b22455930ff | 84 | } |
Roytsg | 28:4b22455930ff | 85 | else if (x==1) { |
Roytsg | 28:4b22455930ff | 86 | x=0; |
Roytsg | 28:4b22455930ff | 87 | } |
Roytsg | 28:4b22455930ff | 88 | } |
Roytsg | 28:4b22455930ff | 89 | |
Roytsg | 28:4b22455930ff | 90 | scope.set(2, movmean); // stuurt de moving average naar de plot |
Roytsg | 28:4b22455930ff | 91 | |
Roytsg | 21:77998ce2c0dd | 92 | scope.send(); |
Roytsg | 21:77998ce2c0dd | 93 | } |
Roytsg | 21:77998ce2c0dd | 94 | |
Roytsg | 21:77998ce2c0dd | 95 | |
Roytsg | 28:4b22455930ff | 96 | |
vsluiter | 0:32bb76391d89 | 97 | int main() |
Roytsg | 28:4b22455930ff | 98 | { |
Roytsg | 28:4b22455930ff | 99 | ledB = 1; |
Roytsg | 28:4b22455930ff | 100 | ledG = 1; |
Roytsg | 28:4b22455930ff | 101 | bqc.add( &bq1 ).add( &bq2 ).add( &bq3 ); // hier wordt het filter gemaakt |
Roytsg | 28:4b22455930ff | 102 | emgSampleTicker.attach( &emgSample, 0.002 ); //dit bepaald de sample frequency en is nu 500 Hz |
Roytsg | 28:4b22455930ff | 103 | |
tomlankhorst | 15:0da764eea774 | 104 | while(1) {} |
vsluiter | 0:32bb76391d89 | 105 | } |