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:
- 4:5ceb8f058874
- Parent:
- 3:be5ac89a0b53
- Child:
- 5:e7253884c2e4
- Child:
- 6:1f722bf6a89b
diff -r be5ac89a0b53 -r 5ceb8f058874 main.cpp
--- a/main.cpp Fri Oct 19 08:05:58 2018 +0000
+++ b/main.cpp Wed Oct 24 11:42:35 2018 +0000
@@ -1,71 +1,104 @@
#include "mbed.h"
#include "math.h"
#include "BiQuad.h"
-#include "Servo.h"
-//#include "HIDScope.h"
+//#include "Servo.h"
+#include "HIDScope.h"
//Define objects
-AnalogIn emgL( A0 ); // EMG Left Arm
-AnalogIn emgR( A1 ); // EMG Right Arm
+AnalogIn emgL(A0); // EMG Left Arm
+AnalogIn emgR(A1); // EMG Right Arm
+AnalogIn emgS(A2); // EMG Servo spier
DigitalOut ledB(LED_BLUE); // Informative LED for gaining zero and max
DigitalOut ledR(LED_RED);
DigitalOut ledG(LED_GREEN); // Informative LED for gaining zero and max
-DigitalIn TestButton(PTA4); // Button used for gaining zero and max
-DigitalIn zeromax(PTC6); // Button used for switching between zero and max
+DigitalIn CalButton(PTA4); // Button used for gaining zero and max
+DigitalIn zeromax(PTC6); // Button used for switching between zero and max
Ticker emgSampleTicker; // Ticker for sample frequency
-//HIDScope scope( 4 );
-Servo myservo(D13);
+HIDScope scope( 6 );
+//Servo myservo(D13);
DigitalOut motor1direction(D7);
PwmOut motor1control(D6);
- int P= 200; // Number of points for moving average first emg
- int Q = 200; // Number of points for moving average second emg
+ int P= 200; // Number of points for movag first emg
+ int Q = 200; // Number of points for movag second emg
+ int R = 200; // Number of points for movag third emg
double A[200]; // Vector for storing data of first emg
double B[200]; // Vector for storing data of second emg
+ double C[200]; // Vector for storing data of third 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
+ double Lvector[200]; // Vector for Rwaarde configuration
+ double Lwaarde[2]; // Vector for storage of max and zero of left biceps
+ double Rvector[200]; // Vector for Lwaarde configuration
+ double Rwaarde[2]; // Vector for storage of max and zero of right biceps
+ double Svector[200]; // Vector for Swaarde configuration
+ double Swaarde[2]; // Vector for storage of max and zero of servo emg
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
+ double movagR; // Moving Average mean value of right biceps
+ double movagL; // Moving Average mean value of left biceps
+ double movagS; // Moving Average mean value of servo spier
float thresholdL = 10; // Startvalue for threshold, to block signals -
float thresholdR = 10; // - before the zero and max values are calculated
+ float thresholdS = 10; //-------
- // 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
+ //------------Filter parameters----------------------
+
+//Lowpassfilter
+//const double b0LP = 0.0014831498359569692;
+//const double b1LP = 0.0029662996719139385;
+//const double b2LP = 0.0014831498359569692;
+//const double a1LP = -1.918570032544273;
+//const double a2LP = 0.9245026318881009;
+//Highpassfilter Fc = 10 Hz;, Q = 0.5, Fs = 500 Hz
+const double b0HP = 0.8851221317817073;
+const double b1HP = -1.7702442635634146;
+const double b2HP = 0.8851221317817073;
+const double a1HP = -1.7632371847263784;
+const double a2HP = 0.777251342400451;
+//Notchfilter Fc = 50 Hz, Q = 10, Fs = 500 Hz
+const double b0NO = 0.9714498065192796;
+const double b1NO = -1.5718388053127037;
+const double b2NO = 0.9714498065192796;
+const double a1NO = -1.5718388053127037;
+const double a2NO = 0.9428996130385592;
+
+//--------------Filter------------
+//BiQuad LPR( b0LP, b1LP, b2LP, a1LP, a2LP ); //Lowpass filter Biquad
+BiQuad HPR( b0HP, b1HP, b2HP, a1HP, a2HP ); //Highpass filter Biquad
+BiQuad NOR( b0NO, b1NO, b2NO, a1NO, a2NO ); //Notch filter Biquad
+
+//BiQuad LPL( b0LP, b1LP, b2LP, a1LP, a2LP ); //Lowpass filter Biquad
+BiQuad HPL( b0HP, b1HP, b2HP, a1HP, a2HP ); //Highpass filter Biquad
+BiQuad NOL( b0NO, b1NO, b2NO, a1NO, a2NO ); //Notch filter Biquad
+
+//BiQuad LPS( b0LP, b1LP, b2LP, a1LP, a2LP ); //Lowpass filter Biquad
+BiQuad HPS( b0HP, b1HP, b2HP, a1HP, a2HP ); //Highpass filter Biquad
+BiQuad NOS( b0NO, b1NO, b2NO, a1NO, a2NO ); //Notch filter Biquad
void emgSample() { // EMG function
+
+ double emgNOFilteredL = NOL.step(emgL.read()); // Filtered NO value of EMG signal left biceps
+ double emgHPFilteredL = HPL.step(emgNOFilteredL); // Filtered HP value of EMG signal left biceps
+ double emgabsL = fabs(emgHPFilteredL); // Absolute value of EMG signal left biceps
+
- double emgNOFilteredR = bq3R.step(emgR.read()); // Filtered HP value of EMG signal left biceps
- double emgHPFilteredR = bq2R.step(emgNOFilteredR); // Filtered HP value of EMG signal left biceps
- double emgabsR = fabs(emgHPFilteredR); // Absolute value of EMG signal left biceps
+ double emgNOFilteredR = NOR.step(emgR.read()); // Filtered NO value of EMG signal right biceps
+ double emgHPFilteredR = HPR.step(emgNOFilteredR); // Filtered HP value of EMG signal right biceps
+ double emgabsR = fabs(emgHPFilteredR); // Absolute value of EMG signal right biceps
- double emgNOFilteredL = bq3L.step(emgL.read()); // Filtered HP value of EMG signal left biceps
- double emgHPFilteredL = bq2L.step(emgNOFilteredL); // Filtered HP value of EMG signal left biceps
- double emgabsL = fabs(emgHPFilteredL); // Absolute value of EMG signal left biceps
+
+ double emgNOFilteredS = NOS.step(emgS.read()); // Filtered NO value of EMG signal servo spier
+ double emgHPFilteredS = HPS.step(emgNOFilteredS); // Filtered HP value of EMG signal servo spier
+ double emgabsS = fabs(emgHPFilteredS); // Absolute value of EMG signal servo spier
+//-------------------Linker bicep-------------------------------------
+
for(int i = P-1; i >= 0; i--){ // For-loop used for moving average
if (i == 0) {
A[i] = emgabsL;
@@ -78,10 +111,13 @@
for (int n = 0; n < P-1; n++) { // Summation of array
sumL = sumL + A[n];
}
- movmeanL = sumL/P;
+ movagL = sumL/P;
+
- for(int i = P-1; i >= 0; i--){ // For-loop used for moving average
+//--------------Rechter bicep---------------------------------------
+
+ for(int i = Q-1; i >= 0; i--){ // For-loop used for moving average
if (i == 0) {
B[i] = emgabsR;
}
@@ -91,25 +127,44 @@
}
double sumR = 0;
- for (int n = 0; n < P-1; n++) { // Summation of array
+ for (int n = 0; n < Q-1; n++) { // Summation of array
sumR = sumR + B[n];
}
- movmeanR = sumR/P; //dit is de moving average waarde // Moving average value right biceps
+ movagR = sumR/Q; // Moving average value right biceps
+
+//---------------Servo spier ------------------------------------
+
+ for(int i = R-1; i >= 0; i--){ // For-loop used for moving average
+ if (i == 0) {
+ C[i] = emgabsS;
+ }
+ else {
+ C[i] = C[i-1];
+ }
+ }
+ double sumS = 0;
+ for (int n = 0; n < R-1; n++) { // Summation of array
+ sumS = sumS + C[n];
+ }
+ movagS = sumS/R;
/* 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);
- //scope.set(2, emgR.read());
- //scope.set(3, movmeanR);
+ scope.set(0, emgL.read());
+ scope.set(1, movagL);
+ scope.set(2, emgR.read());
+ scope.set(3, movagR);
+ scope.set(4, emgS.read());
+ scope.set(5, movagS);
/* 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
+ scope.send(); // Moving average value left biceps
- if (TestButton==0 & k<200) { // Loop used for gaining max and zero value
- Lvector[k] = movmeanL;
- Rvector[k] = movmeanR;
+ if (CalButton==0 & k<200) { // Loop used for gaining max and zero value
+ Lvector[k] = movagL;
+ Rvector[k] = movagR;
+ Svector[k] = movagS;
if (x==0){
ledB = !ledB; // SPIER NIET AANSPANNEN BIJ BLAUW
@@ -122,12 +177,15 @@
else if (k==200) { // End of the loop, used for calculation value
double sumY = 0;
double sumZ = 0;
+ double sumX = 0;
for (int n = 0; n < 199; n++) {
sumY = sumY + Lvector[n];
sumZ = sumZ + Rvector[n];
+ sumX = sumX + Svector[n];
}
Lwaarde[x] = sumY/200; // Value of zero for left biceps
- Rwaarde[x] = sumZ/200; // Value of zero for rigth biceps
+ Rwaarde[x] = sumZ/200; // Value of zero for right biceps
+ Swaarde[x] = sumX/200; // Value of zero for Servo spier
k++;
ledB = 1;
ledR = 1;
@@ -147,6 +205,7 @@
{
thresholdL = Lwaarde[0]+(Lwaarde[1]-Lwaarde[0])*(0.25f);
thresholdR = Rwaarde[0]+(Rwaarde[1]-Rwaarde[0])*(0.25f);
+ thresholdS = Swaarde[0]+(Swaarde[1]-Swaarde[0])*(0.25f);
}
}
@@ -158,23 +217,24 @@
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
+Swaarde[0] = 0.01;
while(1) {
- if (movmeanR > thresholdR)
- { myservo = 0.5;
- wait(0.01);
- }
- else {
- myservo = 0.0;
- wait(0.01);
- }
-
- if (movmeanL > thresholdL)
- { motor1control.write(0.8);
- motor1direction.write(true);
- }
- else {
- motor1control.write(0);
- }
+ // if (movmeanR > thresholdR)
+// { myservo = 0.5;
+// wait(0.01);
+// }
+// else {
+// myservo = 0.0;
+// wait(0.01);
+// }
+//
+// if (movmeanL > thresholdL)
+// { motor1control.write(0.8);
+// motor1direction.write(true);
+// }
+// else {
+// motor1control.write(0);
+// }
}
}
\ No newline at end of file