to send emg signal to motor with test programm
Dependencies: HIDScope biquadFilter mbed
Fork of EMGvoorjan by
Diff: main.cpp
- Revision:
- 36:7d7c16c688df
- Parent:
- 35:5a8a7bd8ae59
- Child:
- 37:c0f6e3b44d7b
--- a/main.cpp Thu Oct 26 10:05:07 2017 +0000 +++ b/main.cpp Mon Oct 30 11:59:02 2017 +0000 @@ -24,50 +24,50 @@ double Vvector[200]; // vector voor de Vwaarde configuratie double Vwaarde[2]; // vector voor waardes van V int x = 0; // x waarde voor de Vwaarde - double movmean; + double movmeanR; //moving average for the right arm (first emg-shield) int MotorLock = 0; - double EMGInput; + double EMGInputR; float ErrorZero = 3; double MotorSpeed = 0.5; // Filters -BiQuadChain bqc; -BiQuad bq1( 0.6844323315947305,1.368864663189461, 0.6844323315947305,1.2243497755555954,0.5133795508233265); //lp? -BiQuad bq2( 0.6844323315947306, -1.3688646631894612, 0.6844323315947306, -1.2243497755555959, 0.5133795508233266); //hp? -BiQuad bq3( 0.7566897754116633, -1.2243497755555959, 0.7566897754116633, -1.2243497755555959, 0.5133795508233266); // notch? +BiQuadChain bqcR; +BiQuad bq1R( 0.6844323315947305,1.368864663189461, 0.6844323315947305,1.2243497755555954,0.5133795508233265); //lp? +BiQuad bq2R( 0.6844323315947306, -1.3688646631894612, 0.6844323315947306, -1.2243497755555959, 0.5133795508233266); //hp? +BiQuad bq3R( 0.7566897754116633, -1.2243497755555959, 0.7566897754116633, -1.2243497755555959, 0.5133795508233266); // notch? // sample function voor plotten van de emg signalen en moving average void emgSample() { - double emgFiltered = bqc.step( emg.read() ); // gefilterde waarde van het emg signaal - double emgabs = abs(emgFiltered); // absolute waarde van het gefilterde signaal - scope.set(0, emgFiltered ); // stuurt de waarden naar de grafiek - scope.set(1, emgabs ); // stuurt de waarden naar de grafiek + double emgFilteredR = bqcR.step( emg.read() ); // gefilterde waarde van het emg signaal + double emgabsR = abs(emgFilteredR); // absolute waarde van het gefilterde signaal + scope.set(0, emgFilteredR ); // stuurt de waarden naar de grafiek + scope.set(1, emgabsR ); // stuurt de waarden naar de grafiek // deze for-loop maakt de vector voor de moving average for(int i = P-1; i >= 0; i--){ if (i == 0) { - A[i] = emgabs; + A[i] = emgabsR; } else { A[i] = A[i-1]; } } - double sum = 0; + double sumR = 0; // deze for-loop sommeert de array for (int n = 0; n < P-1; n++) { - sum = sum + A[n]; + sumR = sumR + A[n]; } - movmean = sum/P; //dit is de moving average waarde + movmeanR = sumR/P; //dit is de moving average waarde // hier wordt het test programma opgestart, zodat zero waarde kan worden gekregen if (TestButton==0 & k<200) { - Vvector[k] = movmean; + Vvector[k] = movmeanR; ledB = !ledB; k++; } @@ -95,20 +95,20 @@ x=0; } } - scope.set(2, movmean); // stuurt de moving average naar de plot + scope.set(2, movmeanR); // stuurt de moving average naar de plot - if (movmean > Vwaarde[1]) { - movmean = Vwaarde[1]; // zorgt ervoor dat emg nooit > 1 kan zijn + if (movmeanR > Vwaarde[1]) { + movmeanR = Vwaarde[1]; // zorgt ervoor dat emg nooit > 1 kan zijn } - double EMGInputRaw = (movmean - Vwaarde[0]*ErrorZero)/(Vwaarde[1] - Vwaarde[0]*ErrorZero); - if (EMGInputRaw < 0) { - EMGInput = 0; + double EMGInputRawR = (movmeanR - Vwaarde[0]*ErrorZero)/(Vwaarde[1] - Vwaarde[0]*ErrorZero); + if (EMGInputRawR < 0) { + EMGInputR = 0; } else { - EMGInput = EMGInputRaw; + EMGInputR = EMGInputRawR; } - scope.set(5,EMGInput); + scope.set(5,EMGInputR); scope.send(); @@ -121,7 +121,7 @@ // Positive value means clockwise rotation. const float maxVelocity=8.4; // in rad/s of course! float referenceVelocity; // in rad/s - referenceVelocity = (EMGInput * maxVelocity) * MotorLock; + referenceVelocity = (EMGInputR * maxVelocity) * MotorLock; return referenceVelocity; } @@ -154,7 +154,7 @@ { ledB = 1; ledG = 1; -bqc.add( &bq1 ).add( &bq2 ).add( &bq3 ); // hier wordt het filter gemaakt +bqcR.add( &bq1R ).add( &bq2R ).add( &bq3R ); // hier wordt het filter gemaakt emgSampleTicker.attach( &emgSample, 0.002 ); //dit bepaald de sample frequency en is nu 500 Hz Vwaarde[0] = 0.01; motor2DirectionPin = 1; @@ -165,7 +165,7 @@ } else MotorLock = 1; // MotorOn.rise(&MotorLocker); - motor2MagnitudePin = EMGInput*MotorLock; + motor2MagnitudePin = EMGInputR*MotorLock; wait(0.01); } } \ No newline at end of file