![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
kalibratie stappen project EMG
Dependencies: HIDScope MODSERIAL biquadFilter mbed
Fork of Milestone_sample by
main.cpp@22:91ad5984b2f2, 2018-10-24 (annotated)
- Committer:
- MarijkeZondag
- Date:
- Wed Oct 24 19:41:41 2018 +0000
- Revision:
- 22:91ad5984b2f2
- Parent:
- 21:1da43fdbd254
Kalibratie project versie Marijke AF
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
vsluiter | 0:c8f15874531b | 1 | #include "mbed.h" |
vsluiter | 0:c8f15874531b | 2 | #include "MODSERIAL.h" |
MarijkeZondag | 10:39ec51206c8b | 3 | #include <math.h> |
vsluiter | 0:c8f15874531b | 4 | |
MarijkeZondag | 10:39ec51206c8b | 5 | AnalogIn emg0_in (A0); |
MarijkeZondag | 10:39ec51206c8b | 6 | AnalogIn emg1_in (A1); |
MarijkeZondag | 10:39ec51206c8b | 7 | AnalogIn emg2_in (A2); |
MarijkeZondag | 10:39ec51206c8b | 8 | |
MarijkeZondag | 22:91ad5984b2f2 | 9 | InterruptIn button1 (D10); //Is this one available? We need to make a map of which pins are used for what. |
MarijkeZondag | 16:5f7196ddc77b | 10 | InterruptIn button2 (D11); |
MarijkeZondag | 6:f4bbb73f3989 | 11 | |
MarijkeZondag | 22:91ad5984b2f2 | 12 | DigitalOut directionpin1 (D4); //Motor direction pin |
MarijkeZondag | 9:c722418997b5 | 13 | DigitalOut directionpin2 (D7); |
MarijkeZondag | 22:91ad5984b2f2 | 14 | |
MarijkeZondag | 17:741798018c0d | 15 | DigitalOut ledr (LED_RED); |
MarijkeZondag | 17:741798018c0d | 16 | DigitalOut ledb (LED_BLUE); |
MarijkeZondag | 17:741798018c0d | 17 | DigitalOut ledg (LED_GREEN); |
MarijkeZondag | 13:a3d4b4daf5b4 | 18 | |
MarijkeZondag | 22:91ad5984b2f2 | 19 | PwmOut pwmpin1 (D5); //Pulse width modulation --> speed motor |
MarijkeZondag | 9:c722418997b5 | 20 | PwmOut pwmpin2 (D6); |
MarijkeZondag | 9:c722418997b5 | 21 | |
MarijkeZondag | 22:91ad5984b2f2 | 22 | MODSERIAL pc(USBTX, USBRX); //Serial communication to test if the code works |
MarijkeZondag | 22:91ad5984b2f2 | 23 | |
MarijkeZondag | 6:f4bbb73f3989 | 24 | |
MarijkeZondag | 22:91ad5984b2f2 | 25 | //EMG filter variables (also necessary for calibration parts) |
MarijkeZondag | 22:91ad5984b2f2 | 26 | double emg0_filt, emg1_filt, emg2_filt; //Variables for filtered EMG data channel 0, 1 and 2 |
MarijkeZondag | 22:91ad5984b2f2 | 27 | double emg0_raw, emg1_raw, emg2_raw; |
MarijkeZondag | 22:91ad5984b2f2 | 28 | const int windowsize = 150; //Size of the array over which the moving average (MovAg) is calculated. (random number) |
MarijkeZondag | 13:a3d4b4daf5b4 | 29 | double sum, sum1, sum2, sum3; //variables used to sum elements in array |
MarijkeZondag | 22:91ad5984b2f2 | 30 | double StoreArray0[windowsize], StoreArray1[windowsize], StoreArray2[windowsize]; //Empty arrays to calculate MoveAg |
MarijkeZondag | 22:91ad5984b2f2 | 31 | double movAg0, movAg1, movAg2; //outcome of MovAg (moet dit een array zijn??) |
MarijkeZondag | 13:a3d4b4daf5b4 | 32 | |
MarijkeZondag | 22:91ad5984b2f2 | 33 | //Calibration variables |
MarijkeZondag | 22:91ad5984b2f2 | 34 | int x = -1; //Start switch, colour LED is blue. |
MarijkeZondag | 22:91ad5984b2f2 | 35 | int emg_cal = 0; //if emg_cal is set to 1, motors can begin to work in this code (!!) |
MarijkeZondag | 22:91ad5984b2f2 | 36 | const int sizeCal = 2000; //size of the dataset used for calibration |
MarijkeZondag | 22:91ad5984b2f2 | 37 | double StoreCal0[sizeCal], StoreCal1[sizeCal], StoreCal2[sizeCal]; //arrays to put the dataset of the calibration in |
MarijkeZondag | 22:91ad5984b2f2 | 38 | double Mean0,Mean1,Mean2; //average of maximum tightening |
MarijkeZondag | 22:91ad5984b2f2 | 39 | double Threshold0, Threshold1, Threshold2; //Threshold variables |
MarijkeZondag | 10:39ec51206c8b | 40 | |
MarijkeZondag | 9:c722418997b5 | 41 | |
MarijkeZondag | 9:c722418997b5 | 42 | //Functions |
MarijkeZondag | 12:eaed305a76c3 | 43 | |
MarijkeZondag | 13:a3d4b4daf5b4 | 44 | void switch_to_calibrate() |
MarijkeZondag | 13:a3d4b4daf5b4 | 45 | { |
MarijkeZondag | 22:91ad5984b2f2 | 46 | x++; //Every time function gets called, x increases. Every button press --> new calibration state. |
MarijkeZondag | 22:91ad5984b2f2 | 47 | //Starts with x = -1. So when function gets called 1 time, x = 0. In the end, x = 4 will reset to -1. |
MarijkeZondag | 22:91ad5984b2f2 | 48 | |
MarijkeZondag | 13:a3d4b4daf5b4 | 49 | if(x==0) //If x = 0, led is red |
MarijkeZondag | 13:a3d4b4daf5b4 | 50 | { |
MarijkeZondag | 17:741798018c0d | 51 | ledr = 0; |
MarijkeZondag | 17:741798018c0d | 52 | ledb = 1; |
MarijkeZondag | 17:741798018c0d | 53 | ledg = 1; |
MarijkeZondag | 13:a3d4b4daf5b4 | 54 | } |
MarijkeZondag | 13:a3d4b4daf5b4 | 55 | else if (x==1) //If x = 1, led is blue |
MarijkeZondag | 13:a3d4b4daf5b4 | 56 | { |
MarijkeZondag | 17:741798018c0d | 57 | ledr = 1; |
MarijkeZondag | 17:741798018c0d | 58 | ledb = 0; |
MarijkeZondag | 17:741798018c0d | 59 | ledg = 1; |
MarijkeZondag | 13:a3d4b4daf5b4 | 60 | } |
MarijkeZondag | 22:91ad5984b2f2 | 61 | else if (x==2) //If x = 2, led is green |
MarijkeZondag | 13:a3d4b4daf5b4 | 62 | { |
MarijkeZondag | 17:741798018c0d | 63 | ledr = 1; |
MarijkeZondag | 17:741798018c0d | 64 | ledb = 1; |
MarijkeZondag | 17:741798018c0d | 65 | ledg = 0; |
MarijkeZondag | 13:a3d4b4daf5b4 | 66 | } |
MarijkeZondag | 22:91ad5984b2f2 | 67 | else //If x = 3 or 4, led is white |
MarijkeZondag | 13:a3d4b4daf5b4 | 68 | { |
MarijkeZondag | 17:741798018c0d | 69 | ledr = 0; |
MarijkeZondag | 17:741798018c0d | 70 | ledb = 0; |
MarijkeZondag | 17:741798018c0d | 71 | ledg = 0; |
MarijkeZondag | 13:a3d4b4daf5b4 | 72 | } |
MarijkeZondag | 13:a3d4b4daf5b4 | 73 | |
MarijkeZondag | 22:91ad5984b2f2 | 74 | if(x==4) //Reset back to x = -1 |
MarijkeZondag | 13:a3d4b4daf5b4 | 75 | { |
MarijkeZondag | 16:5f7196ddc77b | 76 | x = -1; |
MarijkeZondag | 13:a3d4b4daf5b4 | 77 | } |
MarijkeZondag | 13:a3d4b4daf5b4 | 78 | } |
MarijkeZondag | 13:a3d4b4daf5b4 | 79 | |
MarijkeZondag | 13:a3d4b4daf5b4 | 80 | |
MarijkeZondag | 13:a3d4b4daf5b4 | 81 | void calibrate(void) |
MarijkeZondag | 12:eaed305a76c3 | 82 | { |
MarijkeZondag | 13:a3d4b4daf5b4 | 83 | switch(x) |
MarijkeZondag | 13:a3d4b4daf5b4 | 84 | { |
MarijkeZondag | 22:91ad5984b2f2 | 85 | case 0: //If calibration state 0: |
MarijkeZondag | 13:a3d4b4daf5b4 | 86 | { |
MarijkeZondag | 13:a3d4b4daf5b4 | 87 | sum = 0.0; |
MarijkeZondag | 22:91ad5984b2f2 | 88 | for(int j = 0; j<=sizeCal-1; j++) //Array filled with datapoints from the EMGfilter signal of muscle 0 |
MarijkeZondag | 13:a3d4b4daf5b4 | 89 | { |
MarijkeZondag | 22:91ad5984b2f2 | 90 | StoreCal0[j] = emg0_filt; |
MarijkeZondag | 13:a3d4b4daf5b4 | 91 | sum+=StoreCal0[j]; |
MarijkeZondag | 22:91ad5984b2f2 | 92 | wait(0.001f); //Does there need to be a wait? |
MarijkeZondag | 13:a3d4b4daf5b4 | 93 | } |
MarijkeZondag | 22:91ad5984b2f2 | 94 | Mean0 = sum/sizeCal; //Calculate mean of the datapoints in the calibration set (2000 samples) |
MarijkeZondag | 22:91ad5984b2f2 | 95 | Threshold0 = Mean0/2; //Threshold calculation = 0.5*mean |
MarijkeZondag | 22:91ad5984b2f2 | 96 | break; //Stop. Threshold is calculated, we will use this further in the code |
MarijkeZondag | 13:a3d4b4daf5b4 | 97 | } |
MarijkeZondag | 22:91ad5984b2f2 | 98 | case 1: //If calibration state 1: |
MarijkeZondag | 13:a3d4b4daf5b4 | 99 | { |
MarijkeZondag | 22:91ad5984b2f2 | 100 | sum = 0.0; |
MarijkeZondag | 22:91ad5984b2f2 | 101 | for(int j = 0; j<=sizeCal-1; j++) //Array filled with datapoints from the EMGfilter signal of muscle 1 |
MarijkeZondag | 13:a3d4b4daf5b4 | 102 | { |
MarijkeZondag | 22:91ad5984b2f2 | 103 | StoreCal1[j] = emg1_filt; |
MarijkeZondag | 13:a3d4b4daf5b4 | 104 | sum+=StoreCal1[j]; |
MarijkeZondag | 21:1da43fdbd254 | 105 | wait(0.001f); |
MarijkeZondag | 13:a3d4b4daf5b4 | 106 | } |
MarijkeZondag | 13:a3d4b4daf5b4 | 107 | Mean1 = sum/sizeCal; |
MarijkeZondag | 13:a3d4b4daf5b4 | 108 | Threshold1 = Mean1/2; |
MarijkeZondag | 13:a3d4b4daf5b4 | 109 | break; |
MarijkeZondag | 13:a3d4b4daf5b4 | 110 | } |
MarijkeZondag | 22:91ad5984b2f2 | 111 | case 2: //If calibration state 2: |
MarijkeZondag | 13:a3d4b4daf5b4 | 112 | { |
MarijkeZondag | 13:a3d4b4daf5b4 | 113 | sum = 0.0; |
MarijkeZondag | 22:91ad5984b2f2 | 114 | for(int j = 0; j<=sizeCal-1; j++) //Array filled with datapoints from the EMGfilter signal of muscle 2 |
MarijkeZondag | 13:a3d4b4daf5b4 | 115 | { |
MarijkeZondag | 22:91ad5984b2f2 | 116 | StoreCal1[j] = emg2_filt; |
MarijkeZondag | 13:a3d4b4daf5b4 | 117 | sum+=StoreCal2[j]; |
MarijkeZondag | 21:1da43fdbd254 | 118 | wait(0.001f); |
MarijkeZondag | 13:a3d4b4daf5b4 | 119 | } |
MarijkeZondag | 13:a3d4b4daf5b4 | 120 | Mean2 = sum/sizeCal; |
MarijkeZondag | 13:a3d4b4daf5b4 | 121 | Threshold2 = Mean2/2; |
MarijkeZondag | 13:a3d4b4daf5b4 | 122 | break; |
MarijkeZondag | 13:a3d4b4daf5b4 | 123 | } |
MarijkeZondag | 22:91ad5984b2f2 | 124 | case 3: //EMG is calibrated, robot can be set to Home position. |
MarijkeZondag | 13:a3d4b4daf5b4 | 125 | { |
MarijkeZondag | 22:91ad5984b2f2 | 126 | emg_cal = 1; //This is the setting for which the motors can begin turning in this code (!!) |
MarijkeZondag | 21:1da43fdbd254 | 127 | wait(0.001f); |
MarijkeZondag | 13:a3d4b4daf5b4 | 128 | break; |
MarijkeZondag | 13:a3d4b4daf5b4 | 129 | } |
MarijkeZondag | 22:91ad5984b2f2 | 130 | default: //Ensures nothing happens if x is not 0,1 or 2. |
MarijkeZondag | 13:a3d4b4daf5b4 | 131 | { |
MarijkeZondag | 13:a3d4b4daf5b4 | 132 | break; |
MarijkeZondag | 13:a3d4b4daf5b4 | 133 | } |
MarijkeZondag | 13:a3d4b4daf5b4 | 134 | } |
MarijkeZondag | 12:eaed305a76c3 | 135 | } |
MarijkeZondag | 22:91ad5984b2f2 | 136 | |
MarijkeZondag | 8:895d941a5910 | 137 | |
vsluiter | 0:c8f15874531b | 138 | int main() |
MarijkeZondag | 3:a3ad58758696 | 139 | { |
MarijkeZondag | 22:91ad5984b2f2 | 140 | pc.baud(115200); |
MarijkeZondag | 22:91ad5984b2f2 | 141 | pc.printf("hello\n\r"); //Check does code work |
MarijkeZondag | 10:39ec51206c8b | 142 | |
MarijkeZondag | 22:91ad5984b2f2 | 143 | ledr = 1; //Begin led = blue, press button for first state of calibration --> led will turn red |
MarijkeZondag | 22:91ad5984b2f2 | 144 | ledb = 0; |
MarijkeZondag | 17:741798018c0d | 145 | ledg = 1; |
MarijkeZondag | 22:91ad5984b2f2 | 146 | |
MarijkeZondag | 13:a3d4b4daf5b4 | 147 | button1.rise(switch_to_calibrate); //Switch state of calibration (which muscle) |
MarijkeZondag | 15:be67b090b64a | 148 | wait(0.2f); |
MarijkeZondag | 22:91ad5984b2f2 | 149 | button2.rise(calibrate); //Calibrate threshold for 3 muscles |
MarijkeZondag | 15:be67b090b64a | 150 | wait(0.2f); |
MarijkeZondag | 21:1da43fdbd254 | 151 | |
MarijkeZondag | 13:a3d4b4daf5b4 | 152 | pwmpin1.period_us(60); //60 microseconds PWM period, 16.7 kHz |
vsluiter | 0:c8f15874531b | 153 | |
MarijkeZondag | 22:91ad5984b2f2 | 154 | if(emg_cal==1) //After calibration is finished, emg_cal will be 1. Otherwise 0. |
MarijkeZondag | 16:5f7196ddc77b | 155 | { |
MarijkeZondag | 16:5f7196ddc77b | 156 | while (true) |
MarijkeZondag | 22:91ad5984b2f2 | 157 | { |
MarijkeZondag | 22:91ad5984b2f2 | 158 | |
MarijkeZondag | 22:91ad5984b2f2 | 159 | if(emg0_filt>Threshold0) //If the filtered EMG signal of muscle 0 is higher than the threshold, motor1 will turn in 1 direction |
MarijkeZondag | 16:5f7196ddc77b | 160 | { |
MarijkeZondag | 16:5f7196ddc77b | 161 | pwmpin1 = 1; |
MarijkeZondag | 16:5f7196ddc77b | 162 | directionpin1.write(1); |
MarijkeZondag | 16:5f7196ddc77b | 163 | } |
MarijkeZondag | 22:91ad5984b2f2 | 164 | else //If it is not higher than the threshold, the motor will not turn at all. |
MarijkeZondag | 16:5f7196ddc77b | 165 | { |
MarijkeZondag | 16:5f7196ddc77b | 166 | pwmpin1 = 0; |
MarijkeZondag | 16:5f7196ddc77b | 167 | } |
MarijkeZondag | 9:c722418997b5 | 168 | |
MarijkeZondag | 22:91ad5984b2f2 | 169 | if(emg1_filt>Threshold1) //If the filtered EMG signal of muscle 1 is higher than the threshold, motor2 will turn in 1 direction |
MarijkeZondag | 16:5f7196ddc77b | 170 | { |
MarijkeZondag | 16:5f7196ddc77b | 171 | pwmpin2 = 1; |
MarijkeZondag | 16:5f7196ddc77b | 172 | directionpin2.write(1); |
MarijkeZondag | 16:5f7196ddc77b | 173 | } |
MarijkeZondag | 22:91ad5984b2f2 | 174 | else //If not higher than the threshold, motor will not turn at all |
MarijkeZondag | 16:5f7196ddc77b | 175 | { |
MarijkeZondag | 16:5f7196ddc77b | 176 | pwmpin2 = 0; |
MarijkeZondag | 16:5f7196ddc77b | 177 | } |
MarijkeZondag | 22:91ad5984b2f2 | 178 | if(emg2_filt>Threshold2) //If the filtered EMG signal of muscle 2 is higher than the threshold, motor 1 and 2 will turn |
MarijkeZondag | 16:5f7196ddc77b | 179 | { |
MarijkeZondag | 16:5f7196ddc77b | 180 | pwmpin1 = 1; |
MarijkeZondag | 16:5f7196ddc77b | 181 | pwmpin2 = 2; |
MarijkeZondag | 16:5f7196ddc77b | 182 | directionpin1.write(1); |
MarijkeZondag | 16:5f7196ddc77b | 183 | directionpin2.write(1); |
MarijkeZondag | 16:5f7196ddc77b | 184 | } |
MarijkeZondag | 22:91ad5984b2f2 | 185 | else //If not higher than the threshold, motors will not turn at all |
MarijkeZondag | 16:5f7196ddc77b | 186 | { |
MarijkeZondag | 16:5f7196ddc77b | 187 | pwmpin1 = 0; |
MarijkeZondag | 16:5f7196ddc77b | 188 | pwmpin2 = 0; |
MarijkeZondag | 16:5f7196ddc77b | 189 | } |
MarijkeZondag | 16:5f7196ddc77b | 190 | |
MarijkeZondag | 22:91ad5984b2f2 | 191 | |
MarijkeZondag | 10:39ec51206c8b | 192 | |
MarijkeZondag | 22:91ad5984b2f2 | 193 | } |
MarijkeZondag | 22:91ad5984b2f2 | 194 | } |
MarijkeZondag | 22:91ad5984b2f2 | 195 | |
MarijkeZondag | 22:91ad5984b2f2 | 196 | //Opmerkingen Marijke: ik ben niet zeker van waar ik de while loop moet plaatsen. |
MarijkeZondag | 22:91ad5984b2f2 | 197 | //Misschien is aan het einde buiten de if beter, schrijf dan while(1){}. |
MarijkeZondag | 22:91ad5984b2f2 | 198 | |
MarijkeZondag | 22:91ad5984b2f2 | 199 | //De manier van aansturen van de motoren is nu puur om te testen of de kalibratie werkt. Op deze manier kunnen |
MarijkeZondag | 22:91ad5984b2f2 | 200 | //We namelijk niet met 2 spieren 2 motoren tegelijk aansturen. |
vsluiter | 0:c8f15874531b | 201 | } |