Marijke Zondag / Mbed 2 deprecated Project_script_calibration

kalibratie stappen project EMG

Dependencies:   HIDScope MODSERIAL biquadFilter mbed

Fork of Milestone_sample by Marijke Zondag

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?

UserRevisionLine numberNew 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 }