Biorobotics / Robot-Software

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

help_functions/processing_chain_emg.h@17:1f93c83e211f, 2018-10-25 (annotated)

Committer:
MaikOvermars
Date:
Thu Oct 25 22:01:12 2018 +0000
Branch:
bla
Revision:
17:1f93c83e211f
Parent:
0:4cb1de41d049
Child:
25:734a26538711
Some small bugfixes in kinematics and PID_controller and main. Added the emg processing file. In the main file the processed signals are transformed into velocities.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
MaikOvermars 17:1f93c83e211f 1 #include "mbed.h"
MaikOvermars 17:1f93c83e211f 2 #include "BiQuad.h"
MaikOvermars 17:1f93c83e211f 3
MaikOvermars 17:1f93c83e211f 4 //Define objects
MaikOvermars 17:1f93c83e211f 5
MaikOvermars 17:1f93c83e211f 6 // filter chains for high pass, low pass and notch filters
MaikOvermars 17:1f93c83e211f 7 BiQuadChain bqc;
MaikOvermars 17:1f93c83e211f 8
MaikOvermars 17:1f93c83e211f 9 // these are for sampling frequency 1000 Hz
MaikOvermars 17:1f93c83e211f 10 BiQuad bqlow( 0.1311, 0.2622, 0.1311, -0.7478, 0.2722);
MaikOvermars 17:1f93c83e211f 11 BiQuad bqhigh( 0.9150, -1.8299, 0.9150, -1.8227, 0.8372);
MaikOvermars 17:1f93c83e211f 12 BiQuad bqnotch( 0.9695, -1.8442, 0.9695, -1.8442, 0.9391);
MaikOvermars 17:1f93c83e211f 13
MaikOvermars 17:1f93c83e211f 14 // old filters
MaikOvermars 17:1f93c83e211f 15 //BiQuad bqlow( 0.3767, 0.7533, 0.3767, 0.3172, 0.1894);
MaikOvermars 17:1f93c83e211f 16 //BiQuad bqhigh( 0.6458, -1.2917, 0.6458, -1.1620, 0.4213);
MaikOvermars 17:1f93c83e211f 17 //BiQuad bqnotch( 0.5, 0, 0.5, 0, 0);
MaikOvermars 17:1f93c83e211f 18
MaikOvermars 17:1f93c83e211f 19 const int k = 300; // number of moving average samples
MaikOvermars 17:1f93c83e211f 20 double movavg0[k]= { }; // array with samples emg0
MaikOvermars 17:1f93c83e211f 21 double movavg1[k]= { }; // array with samples emg1
MaikOvermars 17:1f93c83e211f 22 int n = 0; // counter
MaikOvermars 17:1f93c83e211f 23 bool full = 0; // boolean to check if we have already had 300 samples
MaikOvermars 17:1f93c83e211f 24
MaikOvermars 17:1f93c83e211f 25 void processing_chain_emg(double &raw_emg_0, double &raw_emg_1, double &process_emg_0, double &process_emg_1) {
MaikOvermars 17:1f93c83e211f 26
MaikOvermars 17:1f93c83e211f 27 // we first filter high pass and notch, then rectifier and after that low pass
MaikOvermars 17:1f93c83e211f 28 movavg0[n] = bqlow.step(fabs(bqc.step( raw_emg_0 )));
MaikOvermars 17:1f93c83e211f 29 movavg1[n] = bqlow.step(fabs(bqc.step( raw_emg_1 )));
MaikOvermars 17:1f93c83e211f 30
MaikOvermars 17:1f93c83e211f 31 // moving average
MaikOvermars 17:1f93c83e211f 32 double emgavg0 = 0, emgavg1 = 0;
MaikOvermars 17:1f93c83e211f 33 if(full == 0){
MaikOvermars 17:1f93c83e211f 34 for(int i=0;i<k;i++){
MaikOvermars 17:1f93c83e211f 35 emgavg0 = emgavg0 + movavg0[i];
MaikOvermars 17:1f93c83e211f 36 emgavg1 = emgavg1 + movavg1[i];
MaikOvermars 17:1f93c83e211f 37 }
MaikOvermars 17:1f93c83e211f 38 emgavg0 = emgavg0/(n+1);
MaikOvermars 17:1f93c83e211f 39 emgavg1 = emgavg1/(n+1);
MaikOvermars 17:1f93c83e211f 40 }
MaikOvermars 17:1f93c83e211f 41 else{
MaikOvermars 17:1f93c83e211f 42 for(int i=0;i<k;i++){
MaikOvermars 17:1f93c83e211f 43 emgavg0 = emgavg0 + movavg0[i];
MaikOvermars 17:1f93c83e211f 44 emgavg1 = emgavg1 + movavg1[i];
MaikOvermars 17:1f93c83e211f 45 }
MaikOvermars 17:1f93c83e211f 46 emgavg0 = emgavg0/k;
MaikOvermars 17:1f93c83e211f 47 emgavg1 = emgavg1/k;
MaikOvermars 17:1f93c83e211f 48 }
MaikOvermars 17:1f93c83e211f 49 n++;
MaikOvermars 17:1f93c83e211f 50 if(n == k){
MaikOvermars 17:1f93c83e211f 51 n = 0; // reset counter when end array reached
MaikOvermars 17:1f93c83e211f 52 full = 1; // assures we are past 300 samples
MaikOvermars 17:1f93c83e211f 53 }
MaikOvermars 17:1f93c83e211f 54
MaikOvermars 17:1f93c83e211f 55 // outputs
MaikOvermars 17:1f93c83e211f 56 process_emg_0 = emgavg0;
MaikOvermars 17:1f93c83e211f 57 process_emg_1 = emgavg1;
MaikOvermars 17:1f93c83e211f 58 }
MaikOvermars 17:1f93c83e211f 59