Marijke Zondag / Mbed 2 deprecated Project_script

project script

Dependencies:   HIDScope MODSERIAL biquadFilter mbed

Fork of Milestone_sample by Marijke Zondag

main.cpp@20:bd9495b31f50, 2018-10-22 (annotated)

Committer:
MarijkeZondag
Date:
Mon Oct 22 13:00:11 2018 +0000
Revision:
20:bd9495b31f50
Parent:
19:466ada92bf65
Child:
21:1da43fdbd254
.

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 10:39ec51206c8b 7 AnalogIn emg0_in (A0);
MarijkeZondag 10:39ec51206c8b 8 AnalogIn emg1_in (A1);
MarijkeZondag 10:39ec51206c8b 9 AnalogIn emg2_in (A2);
MarijkeZondag 10:39ec51206c8b 10
MarijkeZondag 16:5f7196ddc77b 11 InterruptIn button1 (D10); //Let op, is deze niet bezet? En 11? Even checken, als er een error komt, kan het hier zitten.
MarijkeZondag 16:5f7196ddc77b 12 InterruptIn button2 (D11);
MarijkeZondag 9:c722418997b5 13 InterruptIn encoderA (D9);
MarijkeZondag 9:c722418997b5 14 InterruptIn encoderB (D8);
MarijkeZondag 6:f4bbb73f3989 15
MarijkeZondag 9:c722418997b5 16 DigitalOut directionpin1 (D4);
MarijkeZondag 9:c722418997b5 17 DigitalOut directionpin2 (D7);
MarijkeZondag 17:741798018c0d 18 DigitalOut ledr (LED_RED);
MarijkeZondag 17:741798018c0d 19 DigitalOut ledb (LED_BLUE);
MarijkeZondag 17:741798018c0d 20 DigitalOut ledg (LED_GREEN);
MarijkeZondag 13:a3d4b4daf5b4 21
MarijkeZondag 9:c722418997b5 22 PwmOut pwmpin1 (D5);
MarijkeZondag 9:c722418997b5 23 PwmOut pwmpin2 (D6);
MarijkeZondag 9:c722418997b5 24
MarijkeZondag 6:f4bbb73f3989 25
vsluiter 0:c8f15874531b 26 MODSERIAL pc(USBTX, USBRX);
vsluiter 0:c8f15874531b 27
MarijkeZondag 19:466ada92bf65 28 //HIDscope
MarijkeZondag 19:466ada92bf65 29 Ticker sample_timer;
MarijkeZondag 19:466ada92bf65 30 HIDScope scope( 2 );
MarijkeZondag 9:c722418997b5 31
MarijkeZondag 9:c722418997b5 32 //Global variables
MarijkeZondag 13:a3d4b4daf5b4 33 int encoder = 0; //Starting point encoder
MarijkeZondag 12:eaed305a76c3 34 const float T = 0.001f; //Ticker period
MarijkeZondag 10:39ec51206c8b 35
MarijkeZondag 13:a3d4b4daf5b4 36 //EMG filter
MarijkeZondag 13:a3d4b4daf5b4 37 double emgfilter0, emgfilter1, emgfilter2; //Filtered EMG data 0, 1 and 2
MarijkeZondag 14:fa09dae67390 38 const int windowsize = 150; //Size of the array over which the moving average (MovAg) is calculated
MarijkeZondag 13:a3d4b4daf5b4 39 double sum, sum1, sum2, sum3; //variables used to sum elements in array
MarijkeZondag 14:fa09dae67390 40 double StoreArray0[windowsize], StoreArray1[windowsize], StoreArray2[windowsize]; //Empty arrays to calculate MoveAg
MarijkeZondag 13:a3d4b4daf5b4 41 double movAg0,movAg1,movAg2; //outcome of MovAg
MarijkeZondag 13:a3d4b4daf5b4 42
MarijkeZondag 13:a3d4b4daf5b4 43 //calibration
MarijkeZondag 18:898f54c6aa3d 44 static int x = -1;
MarijkeZondag 13:a3d4b4daf5b4 45 int emg_cal = 0;
MarijkeZondag 13:a3d4b4daf5b4 46 const int sizeCal = 2000;
MarijkeZondag 14:fa09dae67390 47 double StoreCal0[sizeCal], StoreCal1[sizeCal], StoreCal2[sizeCal];
MarijkeZondag 14:fa09dae67390 48 double Mean0,Mean1,Mean2;
MarijkeZondag 13:a3d4b4daf5b4 49 double Threshold0 = 1, Threshold1 = 1, Threshold2 = 1;
MarijkeZondag 13:a3d4b4daf5b4 50
MarijkeZondag 13:a3d4b4daf5b4 51
MarijkeZondag 10:39ec51206c8b 52 //Biquad
MarijkeZondag 10:39ec51206c8b 53 BiQuadChain emg0band;
MarijkeZondag 10:39ec51206c8b 54 BiQuad emg0band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
MarijkeZondag 10:39ec51206c8b 55 BiQuad emg0band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
MarijkeZondag 10:39ec51206c8b 56 BiQuad emg0band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );
MarijkeZondag 10:39ec51206c8b 57
MarijkeZondag 10:39ec51206c8b 58 BiQuadChain emg1band;
MarijkeZondag 10:39ec51206c8b 59 BiQuad emg1band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
MarijkeZondag 10:39ec51206c8b 60 BiQuad emg1band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
MarijkeZondag 10:39ec51206c8b 61 BiQuad emg1band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );
MarijkeZondag 10:39ec51206c8b 62
MarijkeZondag 10:39ec51206c8b 63 BiQuadChain emg2band;
MarijkeZondag 10:39ec51206c8b 64 BiQuad emg2band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
MarijkeZondag 10:39ec51206c8b 65 BiQuad emg2band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
MarijkeZondag 10:39ec51206c8b 66 BiQuad emg2band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );
MarijkeZondag 10:39ec51206c8b 67
MarijkeZondag 10:39ec51206c8b 68 BiQuad notch1( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter
MarijkeZondag 14:fa09dae67390 69 BiQuad notch2( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter
MarijkeZondag 14:fa09dae67390 70 BiQuad notch3( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter
MarijkeZondag 10:39ec51206c8b 71
MarijkeZondag 10:39ec51206c8b 72
MarijkeZondag 10:39ec51206c8b 73 //Tickers
MarijkeZondag 12:eaed305a76c3 74 Ticker filter_tick;
MarijkeZondag 12:eaed305a76c3 75 Ticker MovAg_tick;
MarijkeZondag 9:c722418997b5 76
MarijkeZondag 9:c722418997b5 77 //Functions
MarijkeZondag 19:466ada92bf65 78 void sample()
MarijkeZondag 19:466ada92bf65 79 {
MarijkeZondag 19:466ada92bf65 80 /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
MarijkeZondag 19:466ada92bf65 81 scope.set(0, emg0_in.read() );
MarijkeZondag 19:466ada92bf65 82 scope.set(1, emg1_in.read() );
MarijkeZondag 19:466ada92bf65 83 /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels)
MarijkeZondag 19:466ada92bf65 84 * Ensure that enough channels are available (HIDScope scope( 2 ))
MarijkeZondag 19:466ada92bf65 85 * Finally, send all channels to the PC at once */
MarijkeZondag 19:466ada92bf65 86 scope.send();
MarijkeZondag 19:466ada92bf65 87 /* To indicate that the function is working, the LED is toggled */
MarijkeZondag 19:466ada92bf65 88 }
MarijkeZondag 19:466ada92bf65 89
MarijkeZondag 10:39ec51206c8b 90 void EMGFilter0()
MarijkeZondag 10:39ec51206c8b 91 {
MarijkeZondag 11:b95b0e9e1b89 92 double emg0 = emg0_in.read();
MarijkeZondag 11:b95b0e9e1b89 93 double bandpass0 = emg0band.step(emg0);
MarijkeZondag 11:b95b0e9e1b89 94 double absolute0 = fabs(bandpass0);
MarijkeZondag 11:b95b0e9e1b89 95 double notch0 = notch1.step(absolute0);
MarijkeZondag 10:39ec51206c8b 96 }
MarijkeZondag 10:39ec51206c8b 97
MarijkeZondag 10:39ec51206c8b 98 void EMGFilter1()
MarijkeZondag 10:39ec51206c8b 99 {
MarijkeZondag 11:b95b0e9e1b89 100 double emg1 = emg1_in.read();
MarijkeZondag 11:b95b0e9e1b89 101 double bandpass1 = emg1band.step(emg1);
MarijkeZondag 11:b95b0e9e1b89 102 double absolute1 = fabs(bandpass1);
MarijkeZondag 14:fa09dae67390 103 double notch1 = notch2.step(absolute1);
MarijkeZondag 10:39ec51206c8b 104 }
MarijkeZondag 10:39ec51206c8b 105
MarijkeZondag 10:39ec51206c8b 106 void EMGFilter2()
MarijkeZondag 10:39ec51206c8b 107 {
MarijkeZondag 11:b95b0e9e1b89 108 double emg2 = emg2_in.read();
MarijkeZondag 11:b95b0e9e1b89 109 double bandpass2 = emg2band.step(emg2);
MarijkeZondag 11:b95b0e9e1b89 110 double absolute2 = fabs(bandpass2);
MarijkeZondag 14:fa09dae67390 111 double notch2 = notch3.step(absolute2);
MarijkeZondag 10:39ec51206c8b 112 }
MarijkeZondag 10:39ec51206c8b 113
MarijkeZondag 13:a3d4b4daf5b4 114 void MovAg() //Calculate moving average (MovAg)
MarijkeZondag 13:a3d4b4daf5b4 115 {
MarijkeZondag 13:a3d4b4daf5b4 116 for (int i = windowsize-1; i>=0; i--) //Make array of the last datapoints of the filtered signal
MarijkeZondag 13:a3d4b4daf5b4 117 {
MarijkeZondag 13:a3d4b4daf5b4 118 StoreArray0[i] = StoreArray0[i-1];
MarijkeZondag 13:a3d4b4daf5b4 119 StoreArray1[i] = StoreArray1[i-1];
MarijkeZondag 13:a3d4b4daf5b4 120 StoreArray2[i] = StoreArray2[i-1];
MarijkeZondag 13:a3d4b4daf5b4 121 }
MarijkeZondag 13:a3d4b4daf5b4 122
MarijkeZondag 13:a3d4b4daf5b4 123 StoreArray0[0] = emgfilter0; //Stores the latest datapoint in the first element of the array
MarijkeZondag 13:a3d4b4daf5b4 124 StoreArray1[0] = emgfilter1;
MarijkeZondag 13:a3d4b4daf5b4 125 StoreArray2[0] = emgfilter2;
MarijkeZondag 13:a3d4b4daf5b4 126
MarijkeZondag 13:a3d4b4daf5b4 127 sum1 = 0.0;
MarijkeZondag 13:a3d4b4daf5b4 128 sum2 = 0.0;
MarijkeZondag 13:a3d4b4daf5b4 129 sum3 = 0.0;
MarijkeZondag 13:a3d4b4daf5b4 130
MarijkeZondag 13:a3d4b4daf5b4 131 for(int a = 0; a<= windowsize-1; a++) //Sum the elements in the array
MarijkeZondag 13:a3d4b4daf5b4 132 {
MarijkeZondag 13:a3d4b4daf5b4 133 sum1 += StoreArray0[a];
MarijkeZondag 13:a3d4b4daf5b4 134 sum2 += StoreArray1[a];
MarijkeZondag 13:a3d4b4daf5b4 135 sum3 += StoreArray2[a];
MarijkeZondag 13:a3d4b4daf5b4 136 }
MarijkeZondag 13:a3d4b4daf5b4 137
MarijkeZondag 13:a3d4b4daf5b4 138 movAg0 = sum1/windowsize; //calculates an average in the array
MarijkeZondag 13:a3d4b4daf5b4 139 movAg1 = sum2/windowsize;
MarijkeZondag 14:fa09dae67390 140 movAg2 = sum3/windowsize;
MarijkeZondag 13:a3d4b4daf5b4 141 }
MarijkeZondag 13:a3d4b4daf5b4 142
MarijkeZondag 13:a3d4b4daf5b4 143 void emg_filtered() //Call all filter functions
MarijkeZondag 12:eaed305a76c3 144 {
MarijkeZondag 12:eaed305a76c3 145 EMGFilter0();
MarijkeZondag 12:eaed305a76c3 146 EMGFilter1();
MarijkeZondag 12:eaed305a76c3 147 EMGFilter2();
MarijkeZondag 13:a3d4b4daf5b4 148 MovAg();
MarijkeZondag 12:eaed305a76c3 149 }
MarijkeZondag 12:eaed305a76c3 150
MarijkeZondag 13:a3d4b4daf5b4 151 void switch_to_calibrate()
MarijkeZondag 13:a3d4b4daf5b4 152 {
MarijkeZondag 13:a3d4b4daf5b4 153 x++;
MarijkeZondag 13:a3d4b4daf5b4 154
MarijkeZondag 13:a3d4b4daf5b4 155 if(x==0) //If x = 0, led is red
MarijkeZondag 13:a3d4b4daf5b4 156 {
MarijkeZondag 17:741798018c0d 157 ledr = 0;
MarijkeZondag 17:741798018c0d 158 ledb = 1;
MarijkeZondag 17:741798018c0d 159 ledg = 1;
MarijkeZondag 13:a3d4b4daf5b4 160 }
MarijkeZondag 13:a3d4b4daf5b4 161 else if (x==1) //If x = 1, led is blue
MarijkeZondag 13:a3d4b4daf5b4 162 {
MarijkeZondag 17:741798018c0d 163 ledr = 1;
MarijkeZondag 17:741798018c0d 164 ledb = 0;
MarijkeZondag 17:741798018c0d 165 ledg = 1;
MarijkeZondag 13:a3d4b4daf5b4 166 }
MarijkeZondag 13:a3d4b4daf5b4 167 else if (x == 2) //If x = 2, led is green
MarijkeZondag 13:a3d4b4daf5b4 168 {
MarijkeZondag 17:741798018c0d 169 ledr = 1;
MarijkeZondag 17:741798018c0d 170 ledb = 1;
MarijkeZondag 17:741798018c0d 171 ledg = 0;
MarijkeZondag 13:a3d4b4daf5b4 172 }
MarijkeZondag 18:898f54c6aa3d 173 else //If x = 3, led is white
MarijkeZondag 13:a3d4b4daf5b4 174 {
MarijkeZondag 17:741798018c0d 175 ledr = 0;
MarijkeZondag 17:741798018c0d 176 ledb = 0;
MarijkeZondag 17:741798018c0d 177 ledg = 0;
MarijkeZondag 13:a3d4b4daf5b4 178 }
MarijkeZondag 13:a3d4b4daf5b4 179
MarijkeZondag 20:bd9495b31f50 180 if(x==4) //Reset back to x = 0
MarijkeZondag 13:a3d4b4daf5b4 181 {
MarijkeZondag 16:5f7196ddc77b 182 x = -1;
MarijkeZondag 13:a3d4b4daf5b4 183 }
MarijkeZondag 13:a3d4b4daf5b4 184 }
MarijkeZondag 13:a3d4b4daf5b4 185
MarijkeZondag 13:a3d4b4daf5b4 186
MarijkeZondag 13:a3d4b4daf5b4 187 void calibrate(void)
MarijkeZondag 12:eaed305a76c3 188 {
MarijkeZondag 13:a3d4b4daf5b4 189 switch(x)
MarijkeZondag 13:a3d4b4daf5b4 190 {
MarijkeZondag 13:a3d4b4daf5b4 191 case 0:
MarijkeZondag 13:a3d4b4daf5b4 192 {
MarijkeZondag 13:a3d4b4daf5b4 193 sum = 0.0;
MarijkeZondag 13:a3d4b4daf5b4 194 for(int j = 0; j<=sizeCal-1; j++)
MarijkeZondag 13:a3d4b4daf5b4 195 {
MarijkeZondag 13:a3d4b4daf5b4 196 StoreCal0[j] = emgfilter0;
MarijkeZondag 13:a3d4b4daf5b4 197 sum+=StoreCal0[j];
MarijkeZondag 18:898f54c6aa3d 198 //wait(0.001f);
MarijkeZondag 13:a3d4b4daf5b4 199 }
MarijkeZondag 13:a3d4b4daf5b4 200 Mean0 = sum/sizeCal;
MarijkeZondag 13:a3d4b4daf5b4 201 Threshold0 = Mean0/2;
MarijkeZondag 13:a3d4b4daf5b4 202 break;
MarijkeZondag 13:a3d4b4daf5b4 203 }
MarijkeZondag 13:a3d4b4daf5b4 204 case 1:
MarijkeZondag 13:a3d4b4daf5b4 205 {
MarijkeZondag 13:a3d4b4daf5b4 206 sum = 0.0;
MarijkeZondag 13:a3d4b4daf5b4 207 for(int j = 0; j<=sizeCal-1; j++)
MarijkeZondag 13:a3d4b4daf5b4 208 {
MarijkeZondag 13:a3d4b4daf5b4 209 StoreCal1[j] = emgfilter1;
MarijkeZondag 13:a3d4b4daf5b4 210 sum+=StoreCal1[j];
MarijkeZondag 18:898f54c6aa3d 211 //wait(0.001f);
MarijkeZondag 13:a3d4b4daf5b4 212 }
MarijkeZondag 13:a3d4b4daf5b4 213 Mean1 = sum/sizeCal;
MarijkeZondag 13:a3d4b4daf5b4 214 Threshold1 = Mean1/2;
MarijkeZondag 13:a3d4b4daf5b4 215 break;
MarijkeZondag 13:a3d4b4daf5b4 216 }
MarijkeZondag 13:a3d4b4daf5b4 217 case 2:
MarijkeZondag 13:a3d4b4daf5b4 218 {
MarijkeZondag 13:a3d4b4daf5b4 219 sum = 0.0;
MarijkeZondag 13:a3d4b4daf5b4 220 for(int j = 0; j<=sizeCal-1; j++)
MarijkeZondag 13:a3d4b4daf5b4 221 {
MarijkeZondag 13:a3d4b4daf5b4 222 StoreCal1[j] = emgfilter2;
MarijkeZondag 13:a3d4b4daf5b4 223 sum+=StoreCal2[j];
MarijkeZondag 18:898f54c6aa3d 224 //wait(0.001f);
MarijkeZondag 13:a3d4b4daf5b4 225 }
MarijkeZondag 13:a3d4b4daf5b4 226 Mean2 = sum/sizeCal;
MarijkeZondag 13:a3d4b4daf5b4 227 Threshold2 = Mean2/2;
MarijkeZondag 13:a3d4b4daf5b4 228 break;
MarijkeZondag 13:a3d4b4daf5b4 229 }
MarijkeZondag 13:a3d4b4daf5b4 230 case 3: //EMG is calibrated, robot can be set to Home position.
MarijkeZondag 13:a3d4b4daf5b4 231 {
MarijkeZondag 13:a3d4b4daf5b4 232 emg_cal = 1;
MarijkeZondag 18:898f54c6aa3d 233 //wait(0.001f);
MarijkeZondag 13:a3d4b4daf5b4 234 break;
MarijkeZondag 13:a3d4b4daf5b4 235 }
MarijkeZondag 13:a3d4b4daf5b4 236 default: //Ensures nothing happens if x is not 0,1 or 2.
MarijkeZondag 13:a3d4b4daf5b4 237 {
MarijkeZondag 13:a3d4b4daf5b4 238 break;
MarijkeZondag 13:a3d4b4daf5b4 239 }
MarijkeZondag 13:a3d4b4daf5b4 240 }
MarijkeZondag 12:eaed305a76c3 241 }
MarijkeZondag 13:a3d4b4daf5b4 242
MarijkeZondag 8:895d941a5910 243 void encoderA_rise()
MarijkeZondag 8:895d941a5910 244 {
MarijkeZondag 8:895d941a5910 245 if(encoderB==false)
MarijkeZondag 8:895d941a5910 246 {
MarijkeZondag 8:895d941a5910 247 encoder++;
MarijkeZondag 8:895d941a5910 248 }
MarijkeZondag 8:895d941a5910 249 else
MarijkeZondag 8:895d941a5910 250 {
MarijkeZondag 8:895d941a5910 251 encoder--;
MarijkeZondag 8:895d941a5910 252 }
MarijkeZondag 8:895d941a5910 253 }
MarijkeZondag 8:895d941a5910 254
MarijkeZondag 8:895d941a5910 255 void encoderA_fall()
MarijkeZondag 8:895d941a5910 256 {
MarijkeZondag 8:895d941a5910 257 if(encoderB==true)
MarijkeZondag 8:895d941a5910 258 {
MarijkeZondag 8:895d941a5910 259 encoder++;
MarijkeZondag 8:895d941a5910 260 }
MarijkeZondag 8:895d941a5910 261 else
MarijkeZondag 8:895d941a5910 262 {
MarijkeZondag 8:895d941a5910 263 encoder--;
MarijkeZondag 8:895d941a5910 264 }
MarijkeZondag 8:895d941a5910 265 }
MarijkeZondag 8:895d941a5910 266
MarijkeZondag 8:895d941a5910 267 void encoderB_rise()
MarijkeZondag 8:895d941a5910 268 {
MarijkeZondag 8:895d941a5910 269 if(encoderA==true)
MarijkeZondag 8:895d941a5910 270 {
MarijkeZondag 8:895d941a5910 271 encoder++;
MarijkeZondag 8:895d941a5910 272 }
MarijkeZondag 8:895d941a5910 273 else
MarijkeZondag 8:895d941a5910 274 {
MarijkeZondag 8:895d941a5910 275 encoder--;
MarijkeZondag 8:895d941a5910 276 }
MarijkeZondag 8:895d941a5910 277 }
MarijkeZondag 8:895d941a5910 278
MarijkeZondag 8:895d941a5910 279 void encoderB_fall()
MarijkeZondag 8:895d941a5910 280 {
MarijkeZondag 8:895d941a5910 281 if(encoderA==false)
MarijkeZondag 8:895d941a5910 282 {
MarijkeZondag 8:895d941a5910 283 encoder++;
MarijkeZondag 8:895d941a5910 284 }
MarijkeZondag 8:895d941a5910 285 else
MarijkeZondag 8:895d941a5910 286 {
MarijkeZondag 8:895d941a5910 287 encoder--;
MarijkeZondag 8:895d941a5910 288 }
MarijkeZondag 8:895d941a5910 289 }
MarijkeZondag 8:895d941a5910 290
MarijkeZondag 8:895d941a5910 291
MarijkeZondag 9:c722418997b5 292 // Main function start.
MarijkeZondag 8:895d941a5910 293
vsluiter 0:c8f15874531b 294 int main()
MarijkeZondag 3:a3ad58758696 295 {
MarijkeZondag 13:a3d4b4daf5b4 296 //pc.baud(115200);
MarijkeZondag 13:a3d4b4daf5b4 297 //pc.printf("hello\n\r");
MarijkeZondag 19:466ada92bf65 298 sample_timer.attach(&sample, 0.002);
MarijkeZondag 10:39ec51206c8b 299
MarijkeZondag 17:741798018c0d 300 ledr = 0; //Begin led = red, first state of calibration
MarijkeZondag 17:741798018c0d 301 ledb = 1;
MarijkeZondag 17:741798018c0d 302 ledg = 1;
MarijkeZondag 13:a3d4b4daf5b4 303
MarijkeZondag 13:a3d4b4daf5b4 304 filter_tick.attach(&emg_filtered,T); //EMG signals filtered + moving average every T sec.
MarijkeZondag 13:a3d4b4daf5b4 305 button1.rise(switch_to_calibrate); //Switch state of calibration (which muscle)
MarijkeZondag 15:be67b090b64a 306 wait(0.2f);
MarijkeZondag 13:a3d4b4daf5b4 307 button2.rise(calibrate); //calibrate threshold for 3 muscles
MarijkeZondag 15:be67b090b64a 308 wait(0.2f);
MarijkeZondag 13:a3d4b4daf5b4 309
MarijkeZondag 13:a3d4b4daf5b4 310 pwmpin1.period_us(60); //60 microseconds PWM period, 16.7 kHz
MarijkeZondag 6:f4bbb73f3989 311
MarijkeZondag 8:895d941a5910 312 encoderA.rise(&encoderA_rise);
MarijkeZondag 8:895d941a5910 313 encoderA.fall(&encoderA_fall);
MarijkeZondag 8:895d941a5910 314 encoderB.rise(&encoderB_rise);
MarijkeZondag 8:895d941a5910 315 encoderB.fall(&encoderB_fall);
vsluiter 0:c8f15874531b 316
MarijkeZondag 16:5f7196ddc77b 317 if(emg_cal==1)
MarijkeZondag 16:5f7196ddc77b 318 {
MarijkeZondag 16:5f7196ddc77b 319 while (true)
MarijkeZondag 16:5f7196ddc77b 320 {
MarijkeZondag 16:5f7196ddc77b 321 //Motor aansturen en encoder uitlezen
MarijkeZondag 16:5f7196ddc77b 322 //float u1 = potmetervalue1;
MarijkeZondag 16:5f7196ddc77b 323 //float u2 = potmetervalue2;
MarijkeZondag 16:5f7196ddc77b 324
MarijkeZondag 16:5f7196ddc77b 325 //float m1 = ((u1*2.0f)-1.0f);
MarijkeZondag 16:5f7196ddc77b 326 //float m2 = ((u2*2.0f)-1.0f);
MarijkeZondag 16:5f7196ddc77b 327
MarijkeZondag 16:5f7196ddc77b 328 //pwmpin1 = fabs(m1*0.6f)+0.4f; //pwm duty cycle can only be positive, floating, 0.4f is "inefficiënt", dit tellen we erbij op, en keer 0.6 om te corrigeren voor de helling.
MarijkeZondag 16:5f7196ddc77b 329
MarijkeZondag 16:5f7196ddc77b 330 if(emgfilter0>Threshold0)
MarijkeZondag 16:5f7196ddc77b 331 {
MarijkeZondag 16:5f7196ddc77b 332 pwmpin1 = 1;
MarijkeZondag 16:5f7196ddc77b 333 directionpin1.write(1);
MarijkeZondag 16:5f7196ddc77b 334 }
MarijkeZondag 16:5f7196ddc77b 335 else
MarijkeZondag 16:5f7196ddc77b 336 {
MarijkeZondag 16:5f7196ddc77b 337 pwmpin1 = 0;
MarijkeZondag 16:5f7196ddc77b 338 }
MarijkeZondag 9:c722418997b5 339
MarijkeZondag 16:5f7196ddc77b 340 if(emgfilter1>Threshold1)
MarijkeZondag 16:5f7196ddc77b 341 {
MarijkeZondag 16:5f7196ddc77b 342 pwmpin2 = 1;
MarijkeZondag 16:5f7196ddc77b 343 directionpin2.write(1);
MarijkeZondag 16:5f7196ddc77b 344 }
MarijkeZondag 16:5f7196ddc77b 345 else
MarijkeZondag 16:5f7196ddc77b 346 {
MarijkeZondag 16:5f7196ddc77b 347 pwmpin2 = 0;
MarijkeZondag 16:5f7196ddc77b 348 }
MarijkeZondag 16:5f7196ddc77b 349 if(emgfilter2>Threshold2)
MarijkeZondag 16:5f7196ddc77b 350 {
MarijkeZondag 16:5f7196ddc77b 351 pwmpin1 = 1;
MarijkeZondag 16:5f7196ddc77b 352 pwmpin2 = 2;
MarijkeZondag 16:5f7196ddc77b 353 directionpin1.write(1);
MarijkeZondag 16:5f7196ddc77b 354 directionpin2.write(1);
MarijkeZondag 16:5f7196ddc77b 355 }
MarijkeZondag 16:5f7196ddc77b 356 else
MarijkeZondag 16:5f7196ddc77b 357 {
MarijkeZondag 16:5f7196ddc77b 358 pwmpin1 = 0;
MarijkeZondag 16:5f7196ddc77b 359 pwmpin2 = 0;
MarijkeZondag 16:5f7196ddc77b 360 }
MarijkeZondag 16:5f7196ddc77b 361
MarijkeZondag 16:5f7196ddc77b 362 //Indien waar, motor draait rechtsom. Indien niet waar, motor draait linksom.
MarijkeZondag 16:5f7196ddc77b 363 //wait(0.01f); //zodat de code niet oneindig doorgaat.
MarijkeZondag 16:5f7196ddc77b 364 //pwmpin2 = fabs(m2*0.6f)+0.4f;
MarijkeZondag 16:5f7196ddc77b 365 //directionpin2.write(m2>0);
MarijkeZondag 16:5f7196ddc77b 366
MarijkeZondag 16:5f7196ddc77b 367 //float encoderDegrees = float(encoder)*(360.0/8400.0);
MarijkeZondag 16:5f7196ddc77b 368
MarijkeZondag 16:5f7196ddc77b 369 //pc.printf("Encoder count: %f \n\r",encoderDegrees);
MarijkeZondag 10:39ec51206c8b 370
vsluiter 0:c8f15874531b 371 }
vsluiter 0:c8f15874531b 372 }
MarijkeZondag 19:466ada92bf65 373 while(1) {}
MarijkeZondag 16:5f7196ddc77b 374 }