..

Dependencies:   HIDScope MODSERIAL biquadFilter mbed

Committer:
MarijkeZondag
Date:
Tue Nov 13 09:58:42 2018 +0000
Revision:
25:b8e9ee9a1bd8
Parent:
24:6d63ad38fef7
VLD demo 2 spieren, lage snelheid motoren

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
MarijkeZondag 22:5d956c93b3ae 16 InterruptIn button1 (D10); //Is this one available? We need to make a map of which pins are used for what.
MarijkeZondag 16:5f7196ddc77b 17 InterruptIn button2 (D11);
MarijkeZondag 6:f4bbb73f3989 18
MarijkeZondag 23:97a976a8f0fc 19 DigitalOut directionpin1 (D7);
MarijkeZondag 23:97a976a8f0fc 20 DigitalOut directionpin2 (D4);
MarijkeZondag 23:97a976a8f0fc 21 PwmOut pwmpin1 (D6);
MarijkeZondag 23:97a976a8f0fc 22 PwmOut pwmpin2 (D5);
MarijkeZondag 22:5d956c93b3ae 23
MarijkeZondag 17:741798018c0d 24 DigitalOut ledr (LED_RED);
MarijkeZondag 17:741798018c0d 25 DigitalOut ledb (LED_BLUE);
MarijkeZondag 17:741798018c0d 26 DigitalOut ledg (LED_GREEN);
MarijkeZondag 13:a3d4b4daf5b4 27
MarijkeZondag 9:c722418997b5 28
MarijkeZondag 23:97a976a8f0fc 29 MODSERIAL pc(USBTX, USBRX); //Serial communication to see if the code works step by step, turn on if hidscope is off
MarijkeZondag 6:f4bbb73f3989 30
MarijkeZondag 23:97a976a8f0fc 31 //HIDScope scope( 6 ); //HIDScope set to 3x2 channels for 3 muscles, raw data + filtered
vsluiter 0:c8f15874531b 32
MarijkeZondag 22:5d956c93b3ae 33 //Tickers
MarijkeZondag 22:5d956c93b3ae 34 Ticker HIDScope_tick; //Ticker for HIDScope
MarijkeZondag 22:5d956c93b3ae 35 Ticker filter_tick; //Ticker for EMG filter
MarijkeZondag 22:5d956c93b3ae 36 Ticker MovAg_tick; //Ticker to calculate Moving Average
MarijkeZondag 23:97a976a8f0fc 37 Ticker Motor_tick; //Ticker motor aansturen
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 25:b8e9ee9a1bd8 43 double emg0_filt, emg1_filt; //Variables for filtered EMG data channel 0, 1 and 2
MarijkeZondag 25:b8e9ee9a1bd8 44 double emg0_raw, emg1_raw;
MarijkeZondag 25:b8e9ee9a1bd8 45 double emg0_filt_x, emg1_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 25:b8e9ee9a1bd8 47 double sum, sum1, sum2; //variables used to sum elements in array
MarijkeZondag 25:b8e9ee9a1bd8 48 double StoreArray0[windowsize], StoreArray1[windowsize]; //Empty arrays to calculate MoveAg
MarijkeZondag 25:b8e9ee9a1bd8 49 double movAg0, movAg1; //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 25:b8e9ee9a1bd8 55 double StoreCal0[sizeCal], StoreCal1[sizeCal]; //arrays to put the dataset of the calibration in
MarijkeZondag 25:b8e9ee9a1bd8 56 double Mean0,Mean1; //average of maximum tightening
MarijkeZondag 25:b8e9ee9a1bd8 57 double Threshold0, Threshold1;
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 9:c722418997b5 72 //Functions
MarijkeZondag 12:eaed305a76c3 73
MarijkeZondag 13:a3d4b4daf5b4 74 void switch_to_calibrate()
MarijkeZondag 13:a3d4b4daf5b4 75 {
MarijkeZondag 22:5d956c93b3ae 76 x++; //Every time function gets called, x increases. Every button press --> new calibration state.
MarijkeZondag 22:5d956c93b3ae 77 //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 78
MarijkeZondag 13:a3d4b4daf5b4 79 if(x==0) //If x = 0, led is red
MarijkeZondag 13:a3d4b4daf5b4 80 {
MarijkeZondag 17:741798018c0d 81 ledr = 0;
MarijkeZondag 17:741798018c0d 82 ledb = 1;
MarijkeZondag 17:741798018c0d 83 ledg = 1;
MarijkeZondag 13:a3d4b4daf5b4 84 }
MarijkeZondag 13:a3d4b4daf5b4 85 else if (x==1) //If x = 1, led is blue
MarijkeZondag 13:a3d4b4daf5b4 86 {
MarijkeZondag 17:741798018c0d 87 ledr = 1;
MarijkeZondag 17:741798018c0d 88 ledb = 0;
MarijkeZondag 17:741798018c0d 89 ledg = 1;
MarijkeZondag 13:a3d4b4daf5b4 90 }
MarijkeZondag 22:5d956c93b3ae 91 else //If x = 3 or 4, led is white
MarijkeZondag 13:a3d4b4daf5b4 92 {
MarijkeZondag 17:741798018c0d 93 ledr = 0;
MarijkeZondag 17:741798018c0d 94 ledb = 0;
MarijkeZondag 17:741798018c0d 95 ledg = 0;
MarijkeZondag 13:a3d4b4daf5b4 96 }
MarijkeZondag 13:a3d4b4daf5b4 97
MarijkeZondag 25:b8e9ee9a1bd8 98 if(x==3) //Reset back to x = -1
MarijkeZondag 13:a3d4b4daf5b4 99 {
MarijkeZondag 16:5f7196ddc77b 100 x = -1;
MarijkeZondag 23:97a976a8f0fc 101 emg_cal=0; //reset, motors off
MarijkeZondag 13:a3d4b4daf5b4 102 }
MarijkeZondag 13:a3d4b4daf5b4 103 }
MarijkeZondag 13:a3d4b4daf5b4 104
MarijkeZondag 13:a3d4b4daf5b4 105
MarijkeZondag 13:a3d4b4daf5b4 106 void calibrate(void)
MarijkeZondag 12:eaed305a76c3 107 {
MarijkeZondag 13:a3d4b4daf5b4 108 switch(x)
MarijkeZondag 13:a3d4b4daf5b4 109 {
MarijkeZondag 22:5d956c93b3ae 110 case 0: //If calibration state 0:
MarijkeZondag 13:a3d4b4daf5b4 111 {
MarijkeZondag 13:a3d4b4daf5b4 112 sum = 0.0;
MarijkeZondag 22:5d956c93b3ae 113 for(int j = 0; j<=sizeCal-1; j++) //Array filled with datapoints from the EMGfilter signal of muscle 0
MarijkeZondag 13:a3d4b4daf5b4 114 {
MarijkeZondag 22:5d956c93b3ae 115 StoreCal0[j] = emg0_filt;
MarijkeZondag 13:a3d4b4daf5b4 116 sum+=StoreCal0[j];
MarijkeZondag 22:5d956c93b3ae 117 wait(0.001f); //Does there need to be a wait?
MarijkeZondag 13:a3d4b4daf5b4 118 }
MarijkeZondag 22:5d956c93b3ae 119 Mean0 = sum/sizeCal; //Calculate mean of the datapoints in the calibration set (2000 samples)
MarijkeZondag 23:97a976a8f0fc 120 Threshold0 = Mean0/2; //Threshold calculation = 0.5*mean
MarijkeZondag 22:5d956c93b3ae 121 break; //Stop. Threshold is calculated, we will use this further in the code
MarijkeZondag 13:a3d4b4daf5b4 122 }
MarijkeZondag 22:5d956c93b3ae 123 case 1: //If calibration state 1:
MarijkeZondag 13:a3d4b4daf5b4 124 {
MarijkeZondag 22:5d956c93b3ae 125 sum = 0.0;
MarijkeZondag 22:5d956c93b3ae 126 for(int j = 0; j<=sizeCal-1; j++) //Array filled with datapoints from the EMGfilter signal of muscle 1
MarijkeZondag 13:a3d4b4daf5b4 127 {
MarijkeZondag 22:5d956c93b3ae 128 StoreCal1[j] = emg1_filt;
MarijkeZondag 13:a3d4b4daf5b4 129 sum+=StoreCal1[j];
MarijkeZondag 21:1da43fdbd254 130 wait(0.001f);
MarijkeZondag 13:a3d4b4daf5b4 131 }
MarijkeZondag 13:a3d4b4daf5b4 132 Mean1 = sum/sizeCal;
MarijkeZondag 23:97a976a8f0fc 133 Threshold1 = Mean1/2;
MarijkeZondag 13:a3d4b4daf5b4 134 break;
MarijkeZondag 13:a3d4b4daf5b4 135 }
MarijkeZondag 25:b8e9ee9a1bd8 136 case 2: //EMG is calibrated, robot can be set to Home position.
MarijkeZondag 13:a3d4b4daf5b4 137 {
MarijkeZondag 22:5d956c93b3ae 138 emg_cal = 1; //This is the setting for which the motors can begin turning in this code (!!)
MarijkeZondag 21:1da43fdbd254 139 wait(0.001f);
MarijkeZondag 13:a3d4b4daf5b4 140 break;
MarijkeZondag 13:a3d4b4daf5b4 141 }
MarijkeZondag 22:5d956c93b3ae 142 default: //Ensures nothing happens if x is not 0,1 or 2.
MarijkeZondag 13:a3d4b4daf5b4 143 {
MarijkeZondag 13:a3d4b4daf5b4 144 break;
MarijkeZondag 13:a3d4b4daf5b4 145 }
MarijkeZondag 13:a3d4b4daf5b4 146 }
MarijkeZondag 12:eaed305a76c3 147 }
MarijkeZondag 22:5d956c93b3ae 148
MarijkeZondag 22:5d956c93b3ae 149 void EMGFilter0()
MarijkeZondag 22:5d956c93b3ae 150 {
MarijkeZondag 22:5d956c93b3ae 151 emg0_raw = emg0_in.read(); //give name to raw EMG0 data
MarijkeZondag 22:5d956c93b3ae 152 emg0_filt_x = emg0filter.step(emg0_raw); //Use biquad chain to filter raw EMG data
MarijkeZondag 22:5d956c93b3ae 153 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 154 }
MarijkeZondag 22:5d956c93b3ae 155
MarijkeZondag 22:5d956c93b3ae 156 void EMGFilter1()
MarijkeZondag 8:895d941a5910 157 {
MarijkeZondag 22:5d956c93b3ae 158 emg1_raw = emg1_in.read(); //give name to raw EMG1 data
MarijkeZondag 22:5d956c93b3ae 159 emg1_filt_x = emg1filter.step(emg1_raw); //Use biquad chain to filter raw EMG data
MarijkeZondag 22:5d956c93b3ae 160 emg1_filt = abs(emg1_filt_x); //rectifier. LET OP: volgorde filter: band-notch-rectifier. Eerst band-rect-notch.
MarijkeZondag 8:895d941a5910 161 }
MarijkeZondag 22:5d956c93b3ae 162
MarijkeZondag 22:5d956c93b3ae 163 void MovAg() //Calculate moving average (MovAg), klopt nog niet!!
MarijkeZondag 22:5d956c93b3ae 164 {
MarijkeZondag 22:5d956c93b3ae 165 for (int i = windowsize-1; i>=0; i--) //Make arrays for the last datapoints of the filtered signals
MarijkeZondag 8:895d941a5910 166 {
MarijkeZondag 22:5d956c93b3ae 167 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 168 StoreArray1[i] = StoreArray1[i-1];
MarijkeZondag 8:895d941a5910 169 }
MarijkeZondag 22:5d956c93b3ae 170
MarijkeZondag 22:5d956c93b3ae 171 StoreArray0[0] = emg0_filt; //Stores the latest datapoint of the filtered signal in the first element of the array
MarijkeZondag 22:5d956c93b3ae 172 StoreArray1[0] = emg1_filt;
MarijkeZondag 22:5d956c93b3ae 173
MarijkeZondag 22:5d956c93b3ae 174 sum1 = 0.0;
MarijkeZondag 22:5d956c93b3ae 175 sum2 = 0.0;
MarijkeZondag 22:5d956c93b3ae 176
MarijkeZondag 22:5d956c93b3ae 177 for(int a = 0; a<= windowsize-1; a++) //Sums the elements in the arrays
MarijkeZondag 8:895d941a5910 178 {
MarijkeZondag 22:5d956c93b3ae 179 sum1 += StoreArray0[a];
MarijkeZondag 22:5d956c93b3ae 180 sum2 += StoreArray1[a];
MarijkeZondag 8:895d941a5910 181 }
MarijkeZondag 22:5d956c93b3ae 182
MarijkeZondag 22:5d956c93b3ae 183 movAg0 = sum1/windowsize; //calculates an average in the array
MarijkeZondag 22:5d956c93b3ae 184 movAg1 = sum2/windowsize;
MarijkeZondag 22:5d956c93b3ae 185 //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 186 }
MarijkeZondag 8:895d941a5910 187
MarijkeZondag 22:5d956c93b3ae 188 void emg_filtered() //Call all filter functions
MarijkeZondag 8:895d941a5910 189 {
MarijkeZondag 22:5d956c93b3ae 190 EMGFilter0();
MarijkeZondag 22:5d956c93b3ae 191 EMGFilter1();
MarijkeZondag 8:895d941a5910 192 }
MarijkeZondag 8:895d941a5910 193
MarijkeZondag 23:97a976a8f0fc 194 void motor_control()
MarijkeZondag 23:97a976a8f0fc 195 {
MarijkeZondag 23:97a976a8f0fc 196 while(emg_cal==1) //After calibration is finished, emg_cal will be 1. Otherwise 0.
MarijkeZondag 16:5f7196ddc77b 197 {
MarijkeZondag 22:5d956c93b3ae 198
MarijkeZondag 24:6d63ad38fef7 199 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 200 {
MarijkeZondag 25:b8e9ee9a1bd8 201 pwmpin1 = 0.05;
MarijkeZondag 25:b8e9ee9a1bd8 202 directionpin1.write(1); //rotation down
MarijkeZondag 22:5d956c93b3ae 203
MarijkeZondag 24:6d63ad38fef7 204 ledr = 1; //Blue
MarijkeZondag 22:5d956c93b3ae 205 ledb = 0;
MarijkeZondag 22:5d956c93b3ae 206 ledg = 1;
MarijkeZondag 22:5d956c93b3ae 207
MarijkeZondag 16:5f7196ddc77b 208 }
MarijkeZondag 24:6d63ad38fef7 209 else 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 210 {
MarijkeZondag 25:b8e9ee9a1bd8 211 pwmpin2 = 0.05;
MarijkeZondag 25:b8e9ee9a1bd8 212 directionpin2.write(1); //forward translation
MarijkeZondag 24:6d63ad38fef7 213 ledr = 0; //red
MarijkeZondag 22:5d956c93b3ae 214 ledb = 1;
MarijkeZondag 22:5d956c93b3ae 215 ledg = 1;
MarijkeZondag 16:5f7196ddc77b 216 }
MarijkeZondag 24:6d63ad38fef7 217 else //If not higher than the threshold, motors will not turn at all
MarijkeZondag 24:6d63ad38fef7 218 {
MarijkeZondag 24:6d63ad38fef7 219 pwmpin1 = 0; //Motoren doen niets
MarijkeZondag 24:6d63ad38fef7 220 pwmpin2 = 0;
MarijkeZondag 23:97a976a8f0fc 221
MarijkeZondag 24:6d63ad38fef7 222 ledr = 0; //white
MarijkeZondag 24:6d63ad38fef7 223 ledb = 0;
MarijkeZondag 24:6d63ad38fef7 224 ledg = 0;
MarijkeZondag 24:6d63ad38fef7 225 }
MarijkeZondag 22:5d956c93b3ae 226
MarijkeZondag 23:97a976a8f0fc 227 break;
MarijkeZondag 23:97a976a8f0fc 228 }
MarijkeZondag 23:97a976a8f0fc 229 }
MarijkeZondag 23:97a976a8f0fc 230
MarijkeZondag 23:97a976a8f0fc 231
MarijkeZondag 23:97a976a8f0fc 232 int main()
MarijkeZondag 23:97a976a8f0fc 233 {
MarijkeZondag 23:97a976a8f0fc 234 pc.baud(115200);
MarijkeZondag 23:97a976a8f0fc 235 pc.printf("Hello World!\r\n"); //Serial communication only works if hidscope is turned off.
MarijkeZondag 23:97a976a8f0fc 236 pwmpin1.period_us(60); //60 microseconds PWM period, 16.7 kHz
MarijkeZondag 23:97a976a8f0fc 237
MarijkeZondag 23:97a976a8f0fc 238 emg0filter.add( &emg0band1 ).add( &emg0band2 ).add( &emg0band3 ).add( &notch1 ); //attach biquad elements to chain
MarijkeZondag 23:97a976a8f0fc 239 emg1filter.add( &emg1band1 ).add( &emg1band2 ).add( &emg1band3 ).add( &notch2 );
MarijkeZondag 10:39ec51206c8b 240
MarijkeZondag 23:97a976a8f0fc 241 while(true)
MarijkeZondag 23:97a976a8f0fc 242 {
MarijkeZondag 23:97a976a8f0fc 243 filter_tick.attach(&emg_filtered,T); //EMG signals filtered every T sec.
MarijkeZondag 23:97a976a8f0fc 244 MovAg_tick.attach(&MovAg,T); //Moving average calculation every T sec.
MarijkeZondag 23:97a976a8f0fc 245 Motor_tick.attach(&motor_control,T); //Test motor control
MarijkeZondag 23:97a976a8f0fc 246
MarijkeZondag 23:97a976a8f0fc 247 // HIDScope_tick.attach(&HIDScope_sample,T); //EMG signals raw + filtered to HIDScope every T sec.
MarijkeZondag 23:97a976a8f0fc 248
MarijkeZondag 23:97a976a8f0fc 249 button1.rise(switch_to_calibrate); //Switch state of calibration (which muscle)
MarijkeZondag 24:6d63ad38fef7 250 wait(0.2f); //Wait to avoid bouncing of button
MarijkeZondag 23:97a976a8f0fc 251 button2.rise(calibrate); //Calibrate threshold for 3 muscles
MarijkeZondag 24:6d63ad38fef7 252 wait(0.2f); //Wait to avoid bouncing of button
MarijkeZondag 23:97a976a8f0fc 253
MarijkeZondag 23:97a976a8f0fc 254 pc.printf("x is %i\n\r",x);
MarijkeZondag 23:97a976a8f0fc 255 pc.printf("Movag0 = %f , Movag1 = %f, Movag2 = %f \n\r",movAg0, movAg1, movAg2);
MarijkeZondag 23:97a976a8f0fc 256 pc.printf("Thresh0 = %f , Thresh1 = %f, Thresh2 = %f \n\r",Threshold0, Threshold1, Threshold2);
MarijkeZondag 23:97a976a8f0fc 257 //wait(2.0f);
MarijkeZondag 23:97a976a8f0fc 258 }
vsluiter 0:c8f15874531b 259 }