Marleen van Hoorn / Mbed 2 deprecated Project_script

inverse kinematics toegevoegd en tickers samengevoegd tot 1 ticker

Dependencies:   HIDScope MODSERIAL biquadFilter mbed

Fork of Project_script by Marijke Zondag

main.cpp@22:5d956c93b3ae, 2018-10-29 (annotated)

Committer:
MarijkeZondag
Date:
Mon Oct 29 14:58:25 2018 +0000
Revision:
22:5d956c93b3ae
Parent:
21:1da43fdbd254
Child:
23:97a976a8f0fc
samengevoegd calibration EMG;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vsluiter 0:c8f15874531b 1 #include "mbed.h"
vsluiter 0:c8f15874531b 2 #include "MODSERIAL.h"
MarijkeZondag 10:39ec51206c8b 3 #include "BiQuad.h"
MarijkeZondag 10:39ec51206c8b 4 #include "HIDScope.h"
MarijkeZondag 10:39ec51206c8b 5 #include <math.h>
vsluiter 0:c8f15874531b 6
MarijkeZondag 22:5d956c93b3ae 7 //ATTENTION: set mBed to version 151
MarijkeZondag 22:5d956c93b3ae 8 // set QEI to version 0, (gebruiken wij (nog) niet, is voor encoder)
MarijkeZondag 22:5d956c93b3ae 9 // set MODSERIAL to version 44
MarijkeZondag 22:5d956c93b3ae 10 // set HIDScope to version 7
MarijkeZondag 22:5d956c93b3ae 11 // set biquadFilter to version 7
MarijkeZondag 10:39ec51206c8b 12
MarijkeZondag 22:5d956c93b3ae 13 AnalogIn emg0_in (A0); //First raw EMG signal input
MarijkeZondag 22:5d956c93b3ae 14 AnalogIn emg1_in (A1); //Second raw EMG signal input
MarijkeZondag 22:5d956c93b3ae 15 AnalogIn emg2_in (A2); //Third raw EMG signal input
MarijkeZondag 22:5d956c93b3ae 16
MarijkeZondag 22:5d956c93b3ae 17 InterruptIn button1 (D10); //Is this one available? We need to make a map of which pins are used for what.
MarijkeZondag 16:5f7196ddc77b 18 InterruptIn button2 (D11);
MarijkeZondag 6:f4bbb73f3989 19
MarijkeZondag 22:5d956c93b3ae 20 DigitalOut directionpin1 (D4); //Motor direction pin
MarijkeZondag 9:c722418997b5 21 DigitalOut directionpin2 (D7);
MarijkeZondag 22:5d956c93b3ae 22
MarijkeZondag 17:741798018c0d 23 DigitalOut ledr (LED_RED);
MarijkeZondag 17:741798018c0d 24 DigitalOut ledb (LED_BLUE);
MarijkeZondag 17:741798018c0d 25 DigitalOut ledg (LED_GREEN);
MarijkeZondag 13:a3d4b4daf5b4 26
MarijkeZondag 22:5d956c93b3ae 27 PwmOut pwmpin1 (D5); //Pulse width modulation --> speed motor
MarijkeZondag 9:c722418997b5 28 PwmOut pwmpin2 (D6);
MarijkeZondag 9:c722418997b5 29
MarijkeZondag 22:5d956c93b3ae 30 //MODSERIAL pc(USBTX, USBRX); //Serial communication to see if the code works step by step, turn on if hidscope is off
MarijkeZondag 6:f4bbb73f3989 31
MarijkeZondag 22:5d956c93b3ae 32 HIDScope scope( 6 ); //HIDScope set to 3x2 channels for 3 muscles, raw data + filtered
vsluiter 0:c8f15874531b 33
MarijkeZondag 22:5d956c93b3ae 34 //Tickers
MarijkeZondag 22:5d956c93b3ae 35 Ticker HIDScope_tick; //Ticker for HIDScope
MarijkeZondag 22:5d956c93b3ae 36 Ticker filter_tick; //Ticker for EMG filter
MarijkeZondag 22:5d956c93b3ae 37 Ticker MovAg_tick; //Ticker to calculate Moving Average
MarijkeZondag 9:c722418997b5 38
MarijkeZondag 9:c722418997b5 39 //Global variables
MarijkeZondag 22:5d956c93b3ae 40 const float T = 0.002f; //Ticker period
MarijkeZondag 10:39ec51206c8b 41
MarijkeZondag 13:a3d4b4daf5b4 42 //EMG filter
MarijkeZondag 22:5d956c93b3ae 43 double emg0_filt, emg1_filt, emg2_filt; //Variables for filtered EMG data channel 0, 1 and 2
MarijkeZondag 22:5d956c93b3ae 44 double emg0_raw, emg1_raw, emg2_raw;
MarijkeZondag 22:5d956c93b3ae 45 double emg0_filt_x, emg1_filt_x, emg2_filt_x;
MarijkeZondag 22:5d956c93b3ae 46 const int windowsize = 150; //Size of the array over which the moving average (MovAg) is calculated. (random number)
MarijkeZondag 13:a3d4b4daf5b4 47 double sum, sum1, sum2, sum3; //variables used to sum elements in array
MarijkeZondag 22:5d956c93b3ae 48 double StoreArray0[windowsize], StoreArray1[windowsize], StoreArray2[windowsize]; //Empty arrays to calculate MoveAg
MarijkeZondag 22:5d956c93b3ae 49 double movAg0, movAg1, movAg2; //outcome of MovAg (moet dit een array zijn??)
MarijkeZondag 13:a3d4b4daf5b4 50
MarijkeZondag 22:5d956c93b3ae 51 //Calibration variables
MarijkeZondag 22:5d956c93b3ae 52 int x = -1; //Start switch, colour LED is blue.
MarijkeZondag 22:5d956c93b3ae 53 int emg_cal = 0; //if emg_cal is set to 1, motors can begin to work in this code (!!)
MarijkeZondag 22:5d956c93b3ae 54 const int sizeCal = 1500; //size of the dataset used for calibration, eerst 2000
MarijkeZondag 22:5d956c93b3ae 55 double StoreCal0[sizeCal], StoreCal1[sizeCal], StoreCal2[sizeCal]; //arrays to put the dataset of the calibration in
MarijkeZondag 22:5d956c93b3ae 56 double Mean0,Mean1,Mean2; //average of maximum tightening
MarijkeZondag 22:5d956c93b3ae 57 double Threshold0, Threshold1, Threshold2;
MarijkeZondag 13:a3d4b4daf5b4 58
MarijkeZondag 22:5d956c93b3ae 59 //Biquad //Variables for the biquad band filters (alle 3 dezelfde maar je kan niet 3x 'emg0band' aanroepen ofzo)
MarijkeZondag 22:5d956c93b3ae 60 BiQuadChain emg0filter;
MarijkeZondag 10:39ec51206c8b 61 BiQuad emg0band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
MarijkeZondag 10:39ec51206c8b 62 BiQuad emg0band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
MarijkeZondag 10:39ec51206c8b 63 BiQuad emg0band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );
MarijkeZondag 22:5d956c93b3ae 64 BiQuad notch1( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter biquad coefficients
MarijkeZondag 10:39ec51206c8b 65
MarijkeZondag 22:5d956c93b3ae 66 BiQuadChain emg1filter;
MarijkeZondag 10:39ec51206c8b 67 BiQuad emg1band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
MarijkeZondag 10:39ec51206c8b 68 BiQuad emg1band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
MarijkeZondag 10:39ec51206c8b 69 BiQuad emg1band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );
MarijkeZondag 22:5d956c93b3ae 70 BiQuad notch2( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter
MarijkeZondag 10:39ec51206c8b 71
MarijkeZondag 22:5d956c93b3ae 72 BiQuadChain emg2filter;
MarijkeZondag 10:39ec51206c8b 73 BiQuad emg2band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
MarijkeZondag 10:39ec51206c8b 74 BiQuad emg2band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
MarijkeZondag 10:39ec51206c8b 75 BiQuad emg2band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );
MarijkeZondag 14:fa09dae67390 76 BiQuad notch3( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter
MarijkeZondag 10:39ec51206c8b 77
MarijkeZondag 9:c722418997b5 78 //Functions
MarijkeZondag 12:eaed305a76c3 79
MarijkeZondag 13:a3d4b4daf5b4 80 void switch_to_calibrate()
MarijkeZondag 13:a3d4b4daf5b4 81 {
MarijkeZondag 22:5d956c93b3ae 82 x++; //Every time function gets called, x increases. Every button press --> new calibration state.
MarijkeZondag 22:5d956c93b3ae 83 //Starts with x = -1. So when function gets called 1 time, x = 0. In the end, x = 4 will reset to -1.
MarijkeZondag 22:5d956c93b3ae 84
MarijkeZondag 13:a3d4b4daf5b4 85 if(x==0) //If x = 0, led is red
MarijkeZondag 13:a3d4b4daf5b4 86 {
MarijkeZondag 17:741798018c0d 87 ledr = 0;
MarijkeZondag 17:741798018c0d 88 ledb = 1;
MarijkeZondag 17:741798018c0d 89 ledg = 1;
MarijkeZondag 13:a3d4b4daf5b4 90 }
MarijkeZondag 13:a3d4b4daf5b4 91 else if (x==1) //If x = 1, led is blue
MarijkeZondag 13:a3d4b4daf5b4 92 {
MarijkeZondag 17:741798018c0d 93 ledr = 1;
MarijkeZondag 17:741798018c0d 94 ledb = 0;
MarijkeZondag 17:741798018c0d 95 ledg = 1;
MarijkeZondag 13:a3d4b4daf5b4 96 }
MarijkeZondag 22:5d956c93b3ae 97 else if (x==2) //If x = 2, led is green
MarijkeZondag 13:a3d4b4daf5b4 98 {
MarijkeZondag 17:741798018c0d 99 ledr = 1;
MarijkeZondag 17:741798018c0d 100 ledb = 1;
MarijkeZondag 17:741798018c0d 101 ledg = 0;
MarijkeZondag 13:a3d4b4daf5b4 102 }
MarijkeZondag 22:5d956c93b3ae 103 else //If x = 3 or 4, led is white
MarijkeZondag 13:a3d4b4daf5b4 104 {
MarijkeZondag 17:741798018c0d 105 ledr = 0;
MarijkeZondag 17:741798018c0d 106 ledb = 0;
MarijkeZondag 17:741798018c0d 107 ledg = 0;
MarijkeZondag 13:a3d4b4daf5b4 108 }
MarijkeZondag 13:a3d4b4daf5b4 109
MarijkeZondag 22:5d956c93b3ae 110 if(x==4) //Reset back to x = -1
MarijkeZondag 13:a3d4b4daf5b4 111 {
MarijkeZondag 16:5f7196ddc77b 112 x = -1;
MarijkeZondag 13:a3d4b4daf5b4 113 }
MarijkeZondag 13:a3d4b4daf5b4 114 }
MarijkeZondag 13:a3d4b4daf5b4 115
MarijkeZondag 13:a3d4b4daf5b4 116
MarijkeZondag 13:a3d4b4daf5b4 117 void calibrate(void)
MarijkeZondag 12:eaed305a76c3 118 {
MarijkeZondag 13:a3d4b4daf5b4 119 switch(x)
MarijkeZondag 13:a3d4b4daf5b4 120 {
MarijkeZondag 22:5d956c93b3ae 121 case 0: //If calibration state 0:
MarijkeZondag 13:a3d4b4daf5b4 122 {
MarijkeZondag 13:a3d4b4daf5b4 123 sum = 0.0;
MarijkeZondag 22:5d956c93b3ae 124 for(int j = 0; j<=sizeCal-1; j++) //Array filled with datapoints from the EMGfilter signal of muscle 0
MarijkeZondag 13:a3d4b4daf5b4 125 {
MarijkeZondag 22:5d956c93b3ae 126 StoreCal0[j] = emg0_filt;
MarijkeZondag 13:a3d4b4daf5b4 127 sum+=StoreCal0[j];
MarijkeZondag 22:5d956c93b3ae 128 wait(0.001f); //Does there need to be a wait?
MarijkeZondag 13:a3d4b4daf5b4 129 }
MarijkeZondag 22:5d956c93b3ae 130 Mean0 = sum/sizeCal; //Calculate mean of the datapoints in the calibration set (2000 samples)
MarijkeZondag 22:5d956c93b3ae 131 Threshold0 = Mean0/2; //Threshold calculation = 0.5*mean
MarijkeZondag 22:5d956c93b3ae 132 break; //Stop. Threshold is calculated, we will use this further in the code
MarijkeZondag 13:a3d4b4daf5b4 133 }
MarijkeZondag 22:5d956c93b3ae 134 case 1: //If calibration state 1:
MarijkeZondag 13:a3d4b4daf5b4 135 {
MarijkeZondag 22:5d956c93b3ae 136 sum = 0.0;
MarijkeZondag 22:5d956c93b3ae 137 for(int j = 0; j<=sizeCal-1; j++) //Array filled with datapoints from the EMGfilter signal of muscle 1
MarijkeZondag 13:a3d4b4daf5b4 138 {
MarijkeZondag 22:5d956c93b3ae 139 StoreCal1[j] = emg1_filt;
MarijkeZondag 13:a3d4b4daf5b4 140 sum+=StoreCal1[j];
MarijkeZondag 21:1da43fdbd254 141 wait(0.001f);
MarijkeZondag 13:a3d4b4daf5b4 142 }
MarijkeZondag 13:a3d4b4daf5b4 143 Mean1 = sum/sizeCal;
MarijkeZondag 13:a3d4b4daf5b4 144 Threshold1 = Mean1/2;
MarijkeZondag 13:a3d4b4daf5b4 145 break;
MarijkeZondag 13:a3d4b4daf5b4 146 }
MarijkeZondag 22:5d956c93b3ae 147 case 2: //If calibration state 2:
MarijkeZondag 13:a3d4b4daf5b4 148 {
MarijkeZondag 13:a3d4b4daf5b4 149 sum = 0.0;
MarijkeZondag 22:5d956c93b3ae 150 for(int j = 0; j<=sizeCal-1; j++) //Array filled with datapoints from the EMGfilter signal of muscle 2
MarijkeZondag 13:a3d4b4daf5b4 151 {
MarijkeZondag 22:5d956c93b3ae 152 StoreCal1[j] = emg2_filt;
MarijkeZondag 13:a3d4b4daf5b4 153 sum+=StoreCal2[j];
MarijkeZondag 21:1da43fdbd254 154 wait(0.001f);
MarijkeZondag 13:a3d4b4daf5b4 155 }
MarijkeZondag 13:a3d4b4daf5b4 156 Mean2 = sum/sizeCal;
MarijkeZondag 13:a3d4b4daf5b4 157 Threshold2 = Mean2/2;
MarijkeZondag 13:a3d4b4daf5b4 158 break;
MarijkeZondag 13:a3d4b4daf5b4 159 }
MarijkeZondag 22:5d956c93b3ae 160 case 3: //EMG is calibrated, robot can be set to Home position.
MarijkeZondag 13:a3d4b4daf5b4 161 {
MarijkeZondag 22:5d956c93b3ae 162 emg_cal = 1; //This is the setting for which the motors can begin turning in this code (!!)
MarijkeZondag 21:1da43fdbd254 163 wait(0.001f);
MarijkeZondag 13:a3d4b4daf5b4 164 break;
MarijkeZondag 13:a3d4b4daf5b4 165 }
MarijkeZondag 22:5d956c93b3ae 166 default: //Ensures nothing happens if x is not 0,1 or 2.
MarijkeZondag 13:a3d4b4daf5b4 167 {
MarijkeZondag 13:a3d4b4daf5b4 168 break;
MarijkeZondag 13:a3d4b4daf5b4 169 }
MarijkeZondag 13:a3d4b4daf5b4 170 }
MarijkeZondag 12:eaed305a76c3 171 }
MarijkeZondag 22:5d956c93b3ae 172
MarijkeZondag 22:5d956c93b3ae 173 void HIDScope_sample()
MarijkeZondag 22:5d956c93b3ae 174 {
MarijkeZondag 22:5d956c93b3ae 175 /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
MarijkeZondag 22:5d956c93b3ae 176 scope.set(0,emg0_raw);
MarijkeZondag 22:5d956c93b3ae 177 //scope.set(1,emg0_filt);
MarijkeZondag 22:5d956c93b3ae 178 scope.set(1,movAg0); //als moving average werkt
MarijkeZondag 22:5d956c93b3ae 179 scope.set(2,emg1_raw);
MarijkeZondag 22:5d956c93b3ae 180 //scope.set(3,emg1_filt);
MarijkeZondag 22:5d956c93b3ae 181 scope.set(3,movAg1); //als moving average werkt
MarijkeZondag 22:5d956c93b3ae 182 scope.set(4,emg2_raw);
MarijkeZondag 22:5d956c93b3ae 183 //scope.set(5,emg2_filt);
MarijkeZondag 22:5d956c93b3ae 184 scope.set(5,movAg2); //als moving average werkt
MarijkeZondag 22:5d956c93b3ae 185
MarijkeZondag 22:5d956c93b3ae 186 scope.send(); //Send data to HIDScope server
MarijkeZondag 22:5d956c93b3ae 187 }
MarijkeZondag 22:5d956c93b3ae 188
MarijkeZondag 22:5d956c93b3ae 189 void EMGFilter0()
MarijkeZondag 22:5d956c93b3ae 190 {
MarijkeZondag 22:5d956c93b3ae 191 emg0_raw = emg0_in.read(); //give name to raw EMG0 data
MarijkeZondag 22:5d956c93b3ae 192 emg0_filt_x = emg0filter.step(emg0_raw); //Use biquad chain to filter raw EMG data
MarijkeZondag 22:5d956c93b3ae 193 emg0_filt = abs(emg0_filt_x); //rectifier. LET OP: volgorde filter: band-notch-rectifier. Eerst band-rect-notch, stel er komt iets raars uit, dan Notch uit de biquad chain halen en aparte chain voor aanmaken.
MarijkeZondag 22:5d956c93b3ae 194 }
MarijkeZondag 22:5d956c93b3ae 195
MarijkeZondag 22:5d956c93b3ae 196 void EMGFilter1()
MarijkeZondag 8:895d941a5910 197 {
MarijkeZondag 22:5d956c93b3ae 198 emg1_raw = emg1_in.read(); //give name to raw EMG1 data
MarijkeZondag 22:5d956c93b3ae 199 emg1_filt_x = emg1filter.step(emg1_raw); //Use biquad chain to filter raw EMG data
MarijkeZondag 22:5d956c93b3ae 200 emg1_filt = abs(emg1_filt_x); //rectifier. LET OP: volgorde filter: band-notch-rectifier. Eerst band-rect-notch.
MarijkeZondag 8:895d941a5910 201 }
MarijkeZondag 8:895d941a5910 202
MarijkeZondag 22:5d956c93b3ae 203 void EMGFilter2()
MarijkeZondag 8:895d941a5910 204 {
MarijkeZondag 22:5d956c93b3ae 205 emg2_raw = emg2_in.read(); //Give name to raw EMG1 data
MarijkeZondag 22:5d956c93b3ae 206 emg2_filt_x = emg2filter.step(emg2_raw); //Use biquad chain to filter raw EMG data
MarijkeZondag 22:5d956c93b3ae 207 emg2_filt = abs(emg2_filt_x); //Rectifier. LET OP: volgorde filter: band-notch-rectifier.
MarijkeZondag 22:5d956c93b3ae 208 }
MarijkeZondag 22:5d956c93b3ae 209
MarijkeZondag 22:5d956c93b3ae 210 void MovAg() //Calculate moving average (MovAg), klopt nog niet!!
MarijkeZondag 22:5d956c93b3ae 211 {
MarijkeZondag 22:5d956c93b3ae 212 for (int i = windowsize-1; i>=0; i--) //Make arrays for the last datapoints of the filtered signals
MarijkeZondag 8:895d941a5910 213 {
MarijkeZondag 22:5d956c93b3ae 214 StoreArray0[i] = StoreArray0[i-1]; //Shifts the i'th element one place to the right, this makes it "rolling or moving" average.
MarijkeZondag 22:5d956c93b3ae 215 StoreArray1[i] = StoreArray1[i-1];
MarijkeZondag 22:5d956c93b3ae 216 StoreArray2[i] = StoreArray2[i-1];
MarijkeZondag 8:895d941a5910 217 }
MarijkeZondag 22:5d956c93b3ae 218
MarijkeZondag 22:5d956c93b3ae 219 StoreArray0[0] = emg0_filt; //Stores the latest datapoint of the filtered signal in the first element of the array
MarijkeZondag 22:5d956c93b3ae 220 StoreArray1[0] = emg1_filt;
MarijkeZondag 22:5d956c93b3ae 221 StoreArray2[0] = emg2_filt;
MarijkeZondag 22:5d956c93b3ae 222
MarijkeZondag 22:5d956c93b3ae 223 sum1 = 0.0;
MarijkeZondag 22:5d956c93b3ae 224 sum2 = 0.0;
MarijkeZondag 22:5d956c93b3ae 225 sum3 = 0.0;
MarijkeZondag 22:5d956c93b3ae 226
MarijkeZondag 22:5d956c93b3ae 227 for(int a = 0; a<= windowsize-1; a++) //Sums the elements in the arrays
MarijkeZondag 8:895d941a5910 228 {
MarijkeZondag 22:5d956c93b3ae 229 sum1 += StoreArray0[a];
MarijkeZondag 22:5d956c93b3ae 230 sum2 += StoreArray1[a];
MarijkeZondag 22:5d956c93b3ae 231 sum3 += StoreArray2[a];
MarijkeZondag 8:895d941a5910 232 }
MarijkeZondag 22:5d956c93b3ae 233
MarijkeZondag 22:5d956c93b3ae 234 movAg0 = sum1/windowsize; //calculates an average in the array
MarijkeZondag 22:5d956c93b3ae 235 movAg1 = sum2/windowsize;
MarijkeZondag 22:5d956c93b3ae 236 movAg2 = sum3/windowsize;
MarijkeZondag 22:5d956c93b3ae 237 //serial getallen sturen, als het 1 getal is gaat hier wat fout, als het een reeks is dan gaat er bij de input naar HIDscope wat fout.
MarijkeZondag 8:895d941a5910 238 }
MarijkeZondag 8:895d941a5910 239
MarijkeZondag 22:5d956c93b3ae 240 void emg_filtered() //Call all filter functions
MarijkeZondag 8:895d941a5910 241 {
MarijkeZondag 22:5d956c93b3ae 242 EMGFilter0();
MarijkeZondag 22:5d956c93b3ae 243 EMGFilter1();
MarijkeZondag 22:5d956c93b3ae 244 EMGFilter2();
MarijkeZondag 8:895d941a5910 245 }
MarijkeZondag 8:895d941a5910 246
MarijkeZondag 8:895d941a5910 247
MarijkeZondag 22:5d956c93b3ae 248 int main()
MarijkeZondag 22:5d956c93b3ae 249 {
MarijkeZondag 22:5d956c93b3ae 250 //pc.baud(115200);
MarijkeZondag 22:5d956c93b3ae 251 //pc.printf("Hello World!\r\n"); //Serial communication only works if hidscope is turned off.
MarijkeZondag 22:5d956c93b3ae 252
MarijkeZondag 22:5d956c93b3ae 253 emg0filter.add( &emg0band1 ).add( &emg0band2 ).add( &emg0band3 ).add( &notch1 ); //attach biquad elements to chain
MarijkeZondag 22:5d956c93b3ae 254 emg1filter.add( &emg1band1 ).add( &emg1band2 ).add( &emg1band3 ).add( &notch2 );
MarijkeZondag 22:5d956c93b3ae 255 emg2filter.add( &emg2band1 ).add( &emg2band2 ).add( &emg2band3 ).add( &notch3 );
MarijkeZondag 8:895d941a5910 256
MarijkeZondag 22:5d956c93b3ae 257 filter_tick.attach(&emg_filtered,T); //EMG signals filtered every T sec.
MarijkeZondag 22:5d956c93b3ae 258 MovAg_tick.attach(&MovAg,T); //Moving average calculation every T sec.
MarijkeZondag 22:5d956c93b3ae 259 HIDScope_tick.attach(&HIDScope_sample,T); //EMG signals raw + filtered to HIDScope every T sec.
MarijkeZondag 10:39ec51206c8b 260
MarijkeZondag 22:5d956c93b3ae 261 ledr = 1; //Begin led = blue, press button for first state of calibration --> led will turn red
MarijkeZondag 22:5d956c93b3ae 262 ledb = 0;
MarijkeZondag 17:741798018c0d 263 ledg = 1;
MarijkeZondag 22:5d956c93b3ae 264
MarijkeZondag 13:a3d4b4daf5b4 265 button1.rise(switch_to_calibrate); //Switch state of calibration (which muscle)
MarijkeZondag 15:be67b090b64a 266 wait(0.2f);
MarijkeZondag 22:5d956c93b3ae 267 button2.rise(calibrate); //Calibrate threshold for 3 muscles
MarijkeZondag 15:be67b090b64a 268 wait(0.2f);
MarijkeZondag 21:1da43fdbd254 269
MarijkeZondag 13:a3d4b4daf5b4 270 pwmpin1.period_us(60); //60 microseconds PWM period, 16.7 kHz
vsluiter 0:c8f15874531b 271
MarijkeZondag 22:5d956c93b3ae 272 if(emg_cal==1) //After calibration is finished, emg_cal will be 1. Otherwise 0.
MarijkeZondag 16:5f7196ddc77b 273 {
MarijkeZondag 22:5d956c93b3ae 274 //while (true)
MarijkeZondag 22:5d956c93b3ae 275 // {
MarijkeZondag 22:5d956c93b3ae 276
MarijkeZondag 22:5d956c93b3ae 277 if(movAg0>Threshold0) //If the filtered EMG signal of muscle 0 is higher than the threshold, motor1 will turn in 1 direction
MarijkeZondag 16:5f7196ddc77b 278 {
MarijkeZondag 16:5f7196ddc77b 279 pwmpin1 = 1;
MarijkeZondag 16:5f7196ddc77b 280 directionpin1.write(1);
MarijkeZondag 22:5d956c93b3ae 281
MarijkeZondag 22:5d956c93b3ae 282 ledr = 1; //Blue
MarijkeZondag 22:5d956c93b3ae 283 ledb = 0;
MarijkeZondag 22:5d956c93b3ae 284 ledg = 1;
MarijkeZondag 22:5d956c93b3ae 285
MarijkeZondag 16:5f7196ddc77b 286 }
MarijkeZondag 22:5d956c93b3ae 287 else //If it is not higher than the threshold, the motor will not turn at all.
MarijkeZondag 16:5f7196ddc77b 288 {
MarijkeZondag 16:5f7196ddc77b 289 pwmpin1 = 0;
MarijkeZondag 22:5d956c93b3ae 290 ledr = 0; //white
MarijkeZondag 22:5d956c93b3ae 291 ledb = 0;
MarijkeZondag 22:5d956c93b3ae 292 ledg = 0;
MarijkeZondag 16:5f7196ddc77b 293 }
MarijkeZondag 9:c722418997b5 294
MarijkeZondag 22:5d956c93b3ae 295 if(movAg1>Threshold1) //If the filtered EMG signal of muscle 1 is higher than the threshold, motor2 will turn in 1 direction
MarijkeZondag 16:5f7196ddc77b 296 {
MarijkeZondag 16:5f7196ddc77b 297 pwmpin2 = 1;
MarijkeZondag 16:5f7196ddc77b 298 directionpin2.write(1);
MarijkeZondag 22:5d956c93b3ae 299 ledr = 0; //red
MarijkeZondag 22:5d956c93b3ae 300 ledb = 1;
MarijkeZondag 22:5d956c93b3ae 301 ledg = 1;
MarijkeZondag 16:5f7196ddc77b 302 }
MarijkeZondag 22:5d956c93b3ae 303 else //If not higher than the threshold, motor will not turn at all
MarijkeZondag 16:5f7196ddc77b 304 {
MarijkeZondag 16:5f7196ddc77b 305 pwmpin2 = 0;
MarijkeZondag 22:5d956c93b3ae 306 ledr = 0; //white
MarijkeZondag 22:5d956c93b3ae 307 ledb = 0;
MarijkeZondag 22:5d956c93b3ae 308 ledg = 0;
MarijkeZondag 16:5f7196ddc77b 309 }
MarijkeZondag 22:5d956c93b3ae 310 if(movAg2>Threshold2) //If the filtered EMG signal of muscle 2 is higher than the threshold, motor 1 and 2 will turn
MarijkeZondag 16:5f7196ddc77b 311 {
MarijkeZondag 16:5f7196ddc77b 312 pwmpin1 = 1;
MarijkeZondag 16:5f7196ddc77b 313 pwmpin2 = 2;
MarijkeZondag 16:5f7196ddc77b 314 directionpin1.write(1);
MarijkeZondag 16:5f7196ddc77b 315 directionpin2.write(1);
MarijkeZondag 22:5d956c93b3ae 316
MarijkeZondag 22:5d956c93b3ae 317 ledr = 1; //green
MarijkeZondag 22:5d956c93b3ae 318 ledb = 1;
MarijkeZondag 22:5d956c93b3ae 319 ledg = 0;
MarijkeZondag 16:5f7196ddc77b 320 }
MarijkeZondag 22:5d956c93b3ae 321 else //If not higher than the threshold, motors will not turn at all
MarijkeZondag 16:5f7196ddc77b 322 {
MarijkeZondag 16:5f7196ddc77b 323 pwmpin1 = 0;
MarijkeZondag 16:5f7196ddc77b 324 pwmpin2 = 0;
MarijkeZondag 22:5d956c93b3ae 325
MarijkeZondag 22:5d956c93b3ae 326 ledr = 0; //white
MarijkeZondag 22:5d956c93b3ae 327 ledb = 0;
MarijkeZondag 22:5d956c93b3ae 328 ledg = 0;
MarijkeZondag 16:5f7196ddc77b 329 }
MarijkeZondag 16:5f7196ddc77b 330
MarijkeZondag 22:5d956c93b3ae 331
MarijkeZondag 10:39ec51206c8b 332
MarijkeZondag 22:5d956c93b3ae 333 //}
MarijkeZondag 22:5d956c93b3ae 334 }
vsluiter 0:c8f15874531b 335 }