Biorobotica TIC
/
Demo_TEST3
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: QEI biquadFilter mbed
Fork of
Demo_TEST
by 
Biorobotica TIC
Diff: main.cpp
- Revision:
- 0:df553b18547d
- Child:
- 1:5c3259ecf10a
diff -r 000000000000 -r df553b18547d main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Wed Oct 17 11:19:42 2018 +0000
@@ -0,0 +1,163 @@
+#include "mbed.h"
+#include "math.h"
+#include "BiQuad.h"
+#include "HIDScope.h"
+
+//Define objects
+AnalogIn emgL( A0 ); // EMG Left Arm
+AnalogIn emgR( A1 ); // EMG Right Arm
+DigitalOut ledB(LED_BLUE); // Informative LED for gaining zero and max
+DigitalOut ledG(LED_GREEN); // Informative LED for gaining zero and max
+DigitalIn TestButton(PTA4); // Button used for gaining zero and max
+DigitalIn onoff(PTC6); // Button used for switching between zero and max
+Ticker emgSampleTicker; // Ticker for sample frequency
+DigitalOut signalL(D4); // Output to motorshield, emg left biceps
+DigitalOut signalR(D5);
+HIDScope scope( 2 ); // Output to motorshield, emg right biceps
+
+
+
+ int P= 200; // Number of points for moving average first emg
+ int Q = 200; // Number of points for moving average second emg
+ double A[200]; // Vector for storing data of first emg
+ double B[200]; // Vector for storing data of second emg
+ int k = 0; // Counter for configuration:
+ double Rvector[200]; // Vector for Rwaarde configuration
+ double Rwaarde[2]; // Vector for storage of max and zero of left biceps
+ double Lvector[200]; // Vector for Lwaarde configuration
+ double Lwaarde[2]; // Vector for storage of max and zero of right biceps
+ int x = 0; // Variable for switching between zero and max
+ double movmeanR; // Moving Average mean value of right biceps
+ double movmeanL; // Moving Average mean value of left biceps
+ float thresholdL = 10; // Startvalue for threshold, to block signals -
+ float thresholdR = 10; // - before the zero and max values are calculated
+
+ // Filters
+BiQuadChain bqcR; // BiQuad for signal right biceps
+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
+BiQuadChain bqcL; // BiQuad for signal left biceps
+BiQuad bq1L( 0.6844323315947305,1.368864663189461, 0.6844323315947305,
+ 1.2243497755555954,0.5133795508233265); // LP
+BiQuad bq2L( 0.6844323315947306, -1.3688646631894612, 0.6844323315947306,
+ -1.2243497755555959, 0.5133795508233266); // HP
+BiQuad bq3L( 0.7566897754116633, -1.2243497755555959, 0.7566897754116633,
+ -1.2243497755555959, 0.5133795508233266); // Notch
+
+
+void emgSample() { // EMG function
+
+ double emgFilteredR = bqcR.step( emgR.read() ); // Filtered value of EMG signal right biceps
+ double emgFilteredL = bqcL.step( emgL.read()); // Filtered value of EMG signal left biceps
+ double emgabsR = abs(emgFilteredR); // Absolute value of EMG signal right biceps
+ double emgabsL = abs(emgFilteredL); // Absolute value of EMG signal left biceps
+
+
+ for(int i = P-1; i >= 0; i--){ // For-loop used for moving average
+ if (i == 0) {
+ A[i] = emgabsL;
+ }
+ else {
+ A[i] = A[i-1];
+ }
+ }
+ double sumL = 0;
+ for (int n = 0; n < P-1; n++) { // Summation of array
+ sumL = sumL + A[n];
+ }
+ movmeanL = sumL/P;
+
+ /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
+ scope.set(0, emgL.read() );
+ scope.set(1, movmeanL);
+ /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels)
+ * Ensure that enough channels are available (HIDScope scope( 2 ))
+ * Finally, send all channels to the PC at once */
+ scope.send(); // Moving average value left biceps
+
+ for(int i = P-1; i >= 0; i--){ // For-loop used for moving average
+ if (i == 0) {
+ B[i] = emgabsR;
+ }
+ else {
+ B[i] = B[i-1];
+ }
+ }
+ double sumR = 0;
+
+ for (int n = 0; n < P-1; n++) { // Summation of array
+ sumR = sumR + B[n];
+ }
+ movmeanR = sumR/P; //dit is de moving average waarde // Moving average value right biceps
+
+
+ if (TestButton==0 & k<200) { // Loop used for gaining max and zero value
+ Lvector[k] = movmeanL;
+ Rvector[k] = movmeanR;
+ ledB = !ledB;
+ k++;
+ }
+ else if (k==200) { // End of the loop, used for calculation value
+ double sumY = 0;
+ double sumZ = 0;
+ for (int n = 0; n < 199; n++) {
+ sumY = sumY + Lvector[n];
+ sumZ = sumZ + Rvector[n];
+ }
+ Lwaarde[x] = sumY/200; // Value of zero for left biceps
+ Rwaarde[x] = sumZ/200; // Value of zero for rigth biceps
+ k++;
+ ledB = 1;
+ ledG = !ledG;
+ }
+ else if (k == 201 && onoff ==0) { // Loop used for switching between zero and max
+ ledG = !ledG;
+ k = 0;
+ if (x==0) {
+ x++;
+ }
+ else if (x==1) {
+ x=0;
+ }
+ }
+ if (x==1) // Determining threshold using zero and max
+ {
+ thresholdL = Lwaarde[0]+(Lwaarde[1]-Lwaarde[0])*(0.25f);
+ thresholdR = Rwaarde[0]+(Rwaarde[1]-Rwaarde[0])*(0.25f);
+ }
+
+ if (movmeanL > thresholdL) // Signal sent to the motors
+ {
+ signalL = 1;
+ }
+ else
+ {
+ signalL = 0;
+ }
+
+ if (movmeanR > thresholdR)
+ {
+ signalR = 1;
+ }
+ else
+ {
+ signalR = 0;
+ }
+}
+
+int main()
+{
+ledB = 1;
+ledG = 1;
+bqcR.add( &bq1R ).add( &bq2R ).add( &bq3R ); // Filter for emg signal
+bqcL.add( &bq1L ).add( &bq2L ).add( &bq3L ); // Filter for emg signal
+emgSampleTicker.attach( &emgSample, 0.002 ); // Ticker for EMG function
+Lwaarde[0] = 0.01; // Startvalue for zerovalue, to -
+Rwaarde[0] = 0.01; // - prevent dividing by 0
+ while(1) {
+ }
+}
\ No newline at end of file