Biorobotica TIC / Mbed 2 deprecated Servo

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:   Servo mbed biquadFilter

main.cpp@3:aacea1875b7a, 2018-11-06 (annotated)

Committer:
Hubertus
Date:
Tue Nov 06 11:14:06 2018 +0000
Revision:
3:aacea1875b7a
Parent:
1:93a041e62d7e 
voor de zip

Who changed what in which revision?

UserRevisionLine numberNew contents of line
TimLu 0:98ae3feb778c 1 #include "mbed.h"
TimLu 0:98ae3feb778c 2 #include "Servo.h"
Hubertus 3:aacea1875b7a 3 #include "BiQuad.h"
TimLu 0:98ae3feb778c 4
Hubertus 3:aacea1875b7a 5 Servo myservo(D6);
Hubertus 3:aacea1875b7a 6
Hubertus 3:aacea1875b7a 7 AnalogIn emgS(A0); // EMG Servo spier
Hubertus 3:aacea1875b7a 8 DigitalIn CalButton(PTA4); // Button used for gaining zero and max
Hubertus 3:aacea1875b7a 9 DigitalIn zeromax(PTC6); // Button used for switching between zero and max
Hubertus 3:aacea1875b7a 10
Hubertus 3:aacea1875b7a 11 DigitalOut ledB(LED_BLUE); // Informative LED for gaining zero and max
Hubertus 3:aacea1875b7a 12 DigitalOut ledR(LED_RED);
Hubertus 3:aacea1875b7a 13 DigitalOut ledG(LED_GREEN); // Informative LED for gaining zero and max
Hubertus 3:aacea1875b7a 14
Hubertus 3:aacea1875b7a 15 Ticker emgSampleTicker; // Ticker for sample frequency
Hubertus 3:aacea1875b7a 16
Hubertus 3:aacea1875b7a 17 int R = 200; // Number of points for movag third emg
Hubertus 3:aacea1875b7a 18 double C[200]; // Vector for storing data of third emg
Hubertus 3:aacea1875b7a 19 int k = 0; // Counter for configuration:
Hubertus 3:aacea1875b7a 20 double Svector[200]; // Vector for Swaarde configuration
Hubertus 3:aacea1875b7a 21 double Swaarde[2]; // Vector for storage of max and zero of servo emg
Hubertus 3:aacea1875b7a 22 int x = 0; // Variable for switching between zero and max
Hubertus 3:aacea1875b7a 23 double movagS; // Moving Average mean value of servo spier
Hubertus 3:aacea1875b7a 24 float thresholdS = 10;
Hubertus 3:aacea1875b7a 25
Hubertus 3:aacea1875b7a 26 //Highpassfilter Fc = 10 Hz;, Q = 0.5, Fs = 500 Hz
Hubertus 3:aacea1875b7a 27 const double b0HP = 0.8851221317817073;
Hubertus 3:aacea1875b7a 28 const double b1HP = -1.7702442635634146;
Hubertus 3:aacea1875b7a 29 const double b2HP = 0.8851221317817073;
Hubertus 3:aacea1875b7a 30 const double a1HP = -1.7632371847263784;
Hubertus 3:aacea1875b7a 31 const double a2HP = 0.777251342400451;
Hubertus 3:aacea1875b7a 32 //Notchfilter Fc = 50 Hz, Q = 10, Fs = 500 Hz
Hubertus 3:aacea1875b7a 33 const double b0NO = 0.9714498065192796;
Hubertus 3:aacea1875b7a 34 const double b1NO = -1.5718388053127037;
Hubertus 3:aacea1875b7a 35 const double b2NO = 0.9714498065192796;
Hubertus 3:aacea1875b7a 36 const double a1NO = -1.5718388053127037;
Hubertus 3:aacea1875b7a 37 const double a2NO = 0.9428996130385592;
Hubertus 3:aacea1875b7a 38
Hubertus 3:aacea1875b7a 39 //BiQuad LPS( b0LP, b1LP, b2LP, a1LP, a2LP ); //Lowpass filter Biquad
Hubertus 3:aacea1875b7a 40 BiQuad HPS( b0HP, b1HP, b2HP, a1HP, a2HP ); //Highpass filter Biquad
Hubertus 3:aacea1875b7a 41 BiQuad NOS( b0NO, b1NO, b2NO, a1NO, a2NO ); //Notch filter Biquad
Hubertus 3:aacea1875b7a 42
Hubertus 3:aacea1875b7a 43
Hubertus 3:aacea1875b7a 44 void emgSample() {
Hubertus 3:aacea1875b7a 45
Hubertus 3:aacea1875b7a 46 double emgNOFilteredS = NOS.step(emgS.read()); // Filtered NO value of EMG signal servo spier
Hubertus 3:aacea1875b7a 47 double emgHPFilteredS = HPS.step(emgNOFilteredS); // Filtered HP value of EMG signal servo spier
Hubertus 3:aacea1875b7a 48 double emgabsS = fabs(emgHPFilteredS); // Absolute value of EMG signal servo spier
Hubertus 3:aacea1875b7a 49
Hubertus 3:aacea1875b7a 50 for(int i = R-1; i >= 0; i--){ // For-loop used for moving average
Hubertus 3:aacea1875b7a 51 if (i == 0) {
Hubertus 3:aacea1875b7a 52 C[i] = emgabsS;
Hubertus 3:aacea1875b7a 53 }
Hubertus 3:aacea1875b7a 54 else {
Hubertus 3:aacea1875b7a 55 C[i] = C[i-1];
Hubertus 3:aacea1875b7a 56 }
Hubertus 3:aacea1875b7a 57 }
Hubertus 3:aacea1875b7a 58 double sumS = 0;
Hubertus 3:aacea1875b7a 59 for (int n = 0; n < R-1; n++) { // Summation of array
Hubertus 3:aacea1875b7a 60 sumS = sumS + C[n];
Hubertus 3:aacea1875b7a 61 }
Hubertus 3:aacea1875b7a 62 movagS = sumS/R;
Hubertus 3:aacea1875b7a 63
Hubertus 3:aacea1875b7a 64 if (CalButton==0 & k<200) { // Loop used for gaining max and zero value
Hubertus 3:aacea1875b7a 65 Svector[k] = movagS;
Hubertus 3:aacea1875b7a 66
Hubertus 3:aacea1875b7a 67 if (x==0){
Hubertus 3:aacea1875b7a 68 ledB = !ledB; // SPIER NIET AANSPANNEN BIJ BLAUW
Hubertus 3:aacea1875b7a 69 } // SPIER WEL AANSPANNEN BIJ ROOD
Hubertus 3:aacea1875b7a 70 else if (x==1){
Hubertus 3:aacea1875b7a 71 ledR = !ledR;
Hubertus 3:aacea1875b7a 72 }
Hubertus 3:aacea1875b7a 73 k++;
Hubertus 3:aacea1875b7a 74 }
Hubertus 3:aacea1875b7a 75 else if (k==200) { // End of the loop, used for calculation value
Hubertus 3:aacea1875b7a 76 double sumX = 0;
Hubertus 3:aacea1875b7a 77 for (int n = 0; n < 199; n++) {
Hubertus 3:aacea1875b7a 78 sumX = sumX + Svector[n];
Hubertus 3:aacea1875b7a 79 }
Hubertus 3:aacea1875b7a 80 Swaarde[x] = sumX/200; // Value of zero for Servo spier
Hubertus 3:aacea1875b7a 81 k++;
Hubertus 3:aacea1875b7a 82 ledB = 1;
Hubertus 3:aacea1875b7a 83 ledR = 1;
Hubertus 3:aacea1875b7a 84 ledG = !ledG;
Hubertus 3:aacea1875b7a 85 }
Hubertus 3:aacea1875b7a 86 else if (k == 201 && zeromax ==0) { // Loop used for switching between zero and max
Hubertus 3:aacea1875b7a 87 ledG = !ledG;
Hubertus 3:aacea1875b7a 88 k = 0;
Hubertus 3:aacea1875b7a 89 if (x==0) {
Hubertus 3:aacea1875b7a 90 x++;
Hubertus 3:aacea1875b7a 91 }
Hubertus 3:aacea1875b7a 92 else if (x==1) {
Hubertus 3:aacea1875b7a 93 x=0;
Hubertus 3:aacea1875b7a 94 }
Hubertus 3:aacea1875b7a 95 }
Hubertus 3:aacea1875b7a 96 if (x==1) // Determining threshold using zero and max
Hubertus 3:aacea1875b7a 97 {
Hubertus 3:aacea1875b7a 98 thresholdS = Swaarde[0]+(Swaarde[1]-Swaarde[0])*(0.25f);
Hubertus 3:aacea1875b7a 99 }
Hubertus 3:aacea1875b7a 100 }
Hubertus 1:93a041e62d7e 101
Hubertus 1:93a041e62d7e 102
Hubertus 1:93a041e62d7e 103
TimLu 0:98ae3feb778c 104
TimLu 0:98ae3feb778c 105 int main() {
Hubertus 3:aacea1875b7a 106
Hubertus 3:aacea1875b7a 107 ledB = 1;
Hubertus 3:aacea1875b7a 108 ledG = 1;
Hubertus 3:aacea1875b7a 109 ledR = 1;
Hubertus 3:aacea1875b7a 110 emgSampleTicker.attach( &emgSample, 0.002 ); // Ticker for EMG function
Hubertus 3:aacea1875b7a 111
TimLu 0:98ae3feb778c 112 while(1) {
Hubertus 1:93a041e62d7e 113
Hubertus 3:aacea1875b7a 114 if (movagS > thresholdS)
Hubertus 3:aacea1875b7a 115 { myservo = 0.5;
Hubertus 3:aacea1875b7a 116 ledB = 0;
Hubertus 3:aacea1875b7a 117 wait(0.01);
TimLu 0:98ae3feb778c 118 }
Hubertus 3:aacea1875b7a 119 else {
Hubertus 3:aacea1875b7a 120 myservo = 0.0;
Hubertus 3:aacea1875b7a 121 wait(0.01);
Hubertus 3:aacea1875b7a 122 ledB = 1;
Hubertus 3:aacea1875b7a 123 }
Hubertus 3:aacea1875b7a 124 }
Hubertus 1:93a041e62d7e 125 }
Hubertus 1:93a041e62d7e 126
Hubertus 1:93a041e62d7e 127