Marijke Zondag / Mbed 2 deprecated Milestone_sample

Make 2 motors turn

Dependencies:   MODSERIAL mbed HIDScope biquadFilter

Fork of Minor_test_serial by Marijke Zondag

main.cpp@21:1da43fdbd254, 2018-10-22 (annotated)

Committer:
MarijkeZondag
Date:
Mon Oct 22 13:44:40 2018 +0000
Revision:
21:1da43fdbd254
Parent:
20:bd9495b31f50
Child:
22:8e61050064a9
.

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 21:1da43fdbd254 30 HIDScope scope( 3 );
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 21:1da43fdbd254 44 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 21:1da43fdbd254 83 scope.set(2, emg2_in.read() );
MarijkeZondag 19:466ada92bf65 84 /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels)
MarijkeZondag 19:466ada92bf65 85 * Ensure that enough channels are available (HIDScope scope( 2 ))
MarijkeZondag 19:466ada92bf65 86 * Finally, send all channels to the PC at once */
MarijkeZondag 19:466ada92bf65 87 scope.send();
MarijkeZondag 19:466ada92bf65 88 /* To indicate that the function is working, the LED is toggled */
MarijkeZondag 19:466ada92bf65 89 }
MarijkeZondag 19:466ada92bf65 90
MarijkeZondag 10:39ec51206c8b 91 void EMGFilter0()
MarijkeZondag 10:39ec51206c8b 92 {
MarijkeZondag 11:b95b0e9e1b89 93 double emg0 = emg0_in.read();
MarijkeZondag 11:b95b0e9e1b89 94 double bandpass0 = emg0band.step(emg0);
MarijkeZondag 11:b95b0e9e1b89 95 double absolute0 = fabs(bandpass0);
MarijkeZondag 21:1da43fdbd254 96 double emgfilter0 = notch1.step(absolute0);
MarijkeZondag 10:39ec51206c8b 97 }
MarijkeZondag 10:39ec51206c8b 98
MarijkeZondag 10:39ec51206c8b 99 void EMGFilter1()
MarijkeZondag 10:39ec51206c8b 100 {
MarijkeZondag 11:b95b0e9e1b89 101 double emg1 = emg1_in.read();
MarijkeZondag 11:b95b0e9e1b89 102 double bandpass1 = emg1band.step(emg1);
MarijkeZondag 11:b95b0e9e1b89 103 double absolute1 = fabs(bandpass1);
MarijkeZondag 21:1da43fdbd254 104 double emgfilter1 = notch2.step(absolute1);
MarijkeZondag 10:39ec51206c8b 105 }
MarijkeZondag 10:39ec51206c8b 106
MarijkeZondag 10:39ec51206c8b 107 void EMGFilter2()
MarijkeZondag 10:39ec51206c8b 108 {
MarijkeZondag 11:b95b0e9e1b89 109 double emg2 = emg2_in.read();
MarijkeZondag 11:b95b0e9e1b89 110 double bandpass2 = emg2band.step(emg2);
MarijkeZondag 11:b95b0e9e1b89 111 double absolute2 = fabs(bandpass2);
MarijkeZondag 21:1da43fdbd254 112 double emgfilter2 = notch3.step(absolute2);
MarijkeZondag 10:39ec51206c8b 113 }
MarijkeZondag 10:39ec51206c8b 114
MarijkeZondag 13:a3d4b4daf5b4 115 void MovAg() //Calculate moving average (MovAg)
MarijkeZondag 13:a3d4b4daf5b4 116 {
MarijkeZondag 13:a3d4b4daf5b4 117 for (int i = windowsize-1; i>=0; i--) //Make array of the last datapoints of the filtered signal
MarijkeZondag 13:a3d4b4daf5b4 118 {
MarijkeZondag 13:a3d4b4daf5b4 119 StoreArray0[i] = StoreArray0[i-1];
MarijkeZondag 13:a3d4b4daf5b4 120 StoreArray1[i] = StoreArray1[i-1];
MarijkeZondag 13:a3d4b4daf5b4 121 StoreArray2[i] = StoreArray2[i-1];
MarijkeZondag 13:a3d4b4daf5b4 122 }
MarijkeZondag 13:a3d4b4daf5b4 123
MarijkeZondag 13:a3d4b4daf5b4 124 StoreArray0[0] = emgfilter0; //Stores the latest datapoint in the first element of the array
MarijkeZondag 13:a3d4b4daf5b4 125 StoreArray1[0] = emgfilter1;
MarijkeZondag 13:a3d4b4daf5b4 126 StoreArray2[0] = emgfilter2;
MarijkeZondag 13:a3d4b4daf5b4 127
MarijkeZondag 13:a3d4b4daf5b4 128 sum1 = 0.0;
MarijkeZondag 13:a3d4b4daf5b4 129 sum2 = 0.0;
MarijkeZondag 13:a3d4b4daf5b4 130 sum3 = 0.0;
MarijkeZondag 13:a3d4b4daf5b4 131
MarijkeZondag 13:a3d4b4daf5b4 132 for(int a = 0; a<= windowsize-1; a++) //Sum the elements in the array
MarijkeZondag 13:a3d4b4daf5b4 133 {
MarijkeZondag 13:a3d4b4daf5b4 134 sum1 += StoreArray0[a];
MarijkeZondag 13:a3d4b4daf5b4 135 sum2 += StoreArray1[a];
MarijkeZondag 13:a3d4b4daf5b4 136 sum3 += StoreArray2[a];
MarijkeZondag 13:a3d4b4daf5b4 137 }
MarijkeZondag 13:a3d4b4daf5b4 138
MarijkeZondag 13:a3d4b4daf5b4 139 movAg0 = sum1/windowsize; //calculates an average in the array
MarijkeZondag 13:a3d4b4daf5b4 140 movAg1 = sum2/windowsize;
MarijkeZondag 14:fa09dae67390 141 movAg2 = sum3/windowsize;
MarijkeZondag 13:a3d4b4daf5b4 142 }
MarijkeZondag 13:a3d4b4daf5b4 143
MarijkeZondag 13:a3d4b4daf5b4 144 void emg_filtered() //Call all filter functions
MarijkeZondag 12:eaed305a76c3 145 {
MarijkeZondag 12:eaed305a76c3 146 EMGFilter0();
MarijkeZondag 12:eaed305a76c3 147 EMGFilter1();
MarijkeZondag 12:eaed305a76c3 148 EMGFilter2();
MarijkeZondag 13:a3d4b4daf5b4 149 MovAg();
MarijkeZondag 12:eaed305a76c3 150 }
MarijkeZondag 12:eaed305a76c3 151
MarijkeZondag 13:a3d4b4daf5b4 152 void switch_to_calibrate()
MarijkeZondag 13:a3d4b4daf5b4 153 {
MarijkeZondag 13:a3d4b4daf5b4 154 x++;
MarijkeZondag 13:a3d4b4daf5b4 155
MarijkeZondag 13:a3d4b4daf5b4 156 if(x==0) //If x = 0, led is red
MarijkeZondag 13:a3d4b4daf5b4 157 {
MarijkeZondag 17:741798018c0d 158 ledr = 0;
MarijkeZondag 17:741798018c0d 159 ledb = 1;
MarijkeZondag 17:741798018c0d 160 ledg = 1;
MarijkeZondag 13:a3d4b4daf5b4 161 }
MarijkeZondag 13:a3d4b4daf5b4 162 else if (x==1) //If x = 1, led is blue
MarijkeZondag 13:a3d4b4daf5b4 163 {
MarijkeZondag 17:741798018c0d 164 ledr = 1;
MarijkeZondag 17:741798018c0d 165 ledb = 0;
MarijkeZondag 17:741798018c0d 166 ledg = 1;
MarijkeZondag 13:a3d4b4daf5b4 167 }
MarijkeZondag 13:a3d4b4daf5b4 168 else if (x == 2) //If x = 2, led is green
MarijkeZondag 13:a3d4b4daf5b4 169 {
MarijkeZondag 17:741798018c0d 170 ledr = 1;
MarijkeZondag 17:741798018c0d 171 ledb = 1;
MarijkeZondag 17:741798018c0d 172 ledg = 0;
MarijkeZondag 13:a3d4b4daf5b4 173 }
MarijkeZondag 18:898f54c6aa3d 174 else //If x = 3, led is white
MarijkeZondag 13:a3d4b4daf5b4 175 {
MarijkeZondag 17:741798018c0d 176 ledr = 0;
MarijkeZondag 17:741798018c0d 177 ledb = 0;
MarijkeZondag 17:741798018c0d 178 ledg = 0;
MarijkeZondag 13:a3d4b4daf5b4 179 }
MarijkeZondag 13:a3d4b4daf5b4 180
MarijkeZondag 20:bd9495b31f50 181 if(x==4) //Reset back to x = 0
MarijkeZondag 13:a3d4b4daf5b4 182 {
MarijkeZondag 16:5f7196ddc77b 183 x = -1;
MarijkeZondag 13:a3d4b4daf5b4 184 }
MarijkeZondag 13:a3d4b4daf5b4 185 }
MarijkeZondag 13:a3d4b4daf5b4 186
MarijkeZondag 13:a3d4b4daf5b4 187
MarijkeZondag 13:a3d4b4daf5b4 188 void calibrate(void)
MarijkeZondag 12:eaed305a76c3 189 {
MarijkeZondag 13:a3d4b4daf5b4 190 switch(x)
MarijkeZondag 13:a3d4b4daf5b4 191 {
MarijkeZondag 13:a3d4b4daf5b4 192 case 0:
MarijkeZondag 13:a3d4b4daf5b4 193 {
MarijkeZondag 13:a3d4b4daf5b4 194 sum = 0.0;
MarijkeZondag 13:a3d4b4daf5b4 195 for(int j = 0; j<=sizeCal-1; j++)
MarijkeZondag 13:a3d4b4daf5b4 196 {
MarijkeZondag 13:a3d4b4daf5b4 197 StoreCal0[j] = emgfilter0;
MarijkeZondag 13:a3d4b4daf5b4 198 sum+=StoreCal0[j];
MarijkeZondag 21:1da43fdbd254 199 wait(0.001f);
MarijkeZondag 13:a3d4b4daf5b4 200 }
MarijkeZondag 13:a3d4b4daf5b4 201 Mean0 = sum/sizeCal;
MarijkeZondag 13:a3d4b4daf5b4 202 Threshold0 = Mean0/2;
MarijkeZondag 13:a3d4b4daf5b4 203 break;
MarijkeZondag 13:a3d4b4daf5b4 204 }
MarijkeZondag 13:a3d4b4daf5b4 205 case 1:
MarijkeZondag 13:a3d4b4daf5b4 206 {
MarijkeZondag 13:a3d4b4daf5b4 207 sum = 0.0;
MarijkeZondag 13:a3d4b4daf5b4 208 for(int j = 0; j<=sizeCal-1; j++)
MarijkeZondag 13:a3d4b4daf5b4 209 {
MarijkeZondag 13:a3d4b4daf5b4 210 StoreCal1[j] = emgfilter1;
MarijkeZondag 13:a3d4b4daf5b4 211 sum+=StoreCal1[j];
MarijkeZondag 21:1da43fdbd254 212 wait(0.001f);
MarijkeZondag 13:a3d4b4daf5b4 213 }
MarijkeZondag 13:a3d4b4daf5b4 214 Mean1 = sum/sizeCal;
MarijkeZondag 13:a3d4b4daf5b4 215 Threshold1 = Mean1/2;
MarijkeZondag 13:a3d4b4daf5b4 216 break;
MarijkeZondag 13:a3d4b4daf5b4 217 }
MarijkeZondag 13:a3d4b4daf5b4 218 case 2:
MarijkeZondag 13:a3d4b4daf5b4 219 {
MarijkeZondag 13:a3d4b4daf5b4 220 sum = 0.0;
MarijkeZondag 13:a3d4b4daf5b4 221 for(int j = 0; j<=sizeCal-1; j++)
MarijkeZondag 13:a3d4b4daf5b4 222 {
MarijkeZondag 13:a3d4b4daf5b4 223 StoreCal1[j] = emgfilter2;
MarijkeZondag 13:a3d4b4daf5b4 224 sum+=StoreCal2[j];
MarijkeZondag 21:1da43fdbd254 225 wait(0.001f);
MarijkeZondag 13:a3d4b4daf5b4 226 }
MarijkeZondag 13:a3d4b4daf5b4 227 Mean2 = sum/sizeCal;
MarijkeZondag 13:a3d4b4daf5b4 228 Threshold2 = Mean2/2;
MarijkeZondag 13:a3d4b4daf5b4 229 break;
MarijkeZondag 13:a3d4b4daf5b4 230 }
MarijkeZondag 13:a3d4b4daf5b4 231 case 3: //EMG is calibrated, robot can be set to Home position.
MarijkeZondag 13:a3d4b4daf5b4 232 {
MarijkeZondag 13:a3d4b4daf5b4 233 emg_cal = 1;
MarijkeZondag 21:1da43fdbd254 234 wait(0.001f);
MarijkeZondag 13:a3d4b4daf5b4 235 break;
MarijkeZondag 13:a3d4b4daf5b4 236 }
MarijkeZondag 13:a3d4b4daf5b4 237 default: //Ensures nothing happens if x is not 0,1 or 2.
MarijkeZondag 13:a3d4b4daf5b4 238 {
MarijkeZondag 13:a3d4b4daf5b4 239 break;
MarijkeZondag 13:a3d4b4daf5b4 240 }
MarijkeZondag 13:a3d4b4daf5b4 241 }
MarijkeZondag 12:eaed305a76c3 242 }
MarijkeZondag 13:a3d4b4daf5b4 243
MarijkeZondag 8:895d941a5910 244 void encoderA_rise()
MarijkeZondag 8:895d941a5910 245 {
MarijkeZondag 8:895d941a5910 246 if(encoderB==false)
MarijkeZondag 8:895d941a5910 247 {
MarijkeZondag 8:895d941a5910 248 encoder++;
MarijkeZondag 8:895d941a5910 249 }
MarijkeZondag 8:895d941a5910 250 else
MarijkeZondag 8:895d941a5910 251 {
MarijkeZondag 8:895d941a5910 252 encoder--;
MarijkeZondag 8:895d941a5910 253 }
MarijkeZondag 8:895d941a5910 254 }
MarijkeZondag 8:895d941a5910 255
MarijkeZondag 8:895d941a5910 256 void encoderA_fall()
MarijkeZondag 8:895d941a5910 257 {
MarijkeZondag 8:895d941a5910 258 if(encoderB==true)
MarijkeZondag 8:895d941a5910 259 {
MarijkeZondag 8:895d941a5910 260 encoder++;
MarijkeZondag 8:895d941a5910 261 }
MarijkeZondag 8:895d941a5910 262 else
MarijkeZondag 8:895d941a5910 263 {
MarijkeZondag 8:895d941a5910 264 encoder--;
MarijkeZondag 8:895d941a5910 265 }
MarijkeZondag 8:895d941a5910 266 }
MarijkeZondag 8:895d941a5910 267
MarijkeZondag 8:895d941a5910 268 void encoderB_rise()
MarijkeZondag 8:895d941a5910 269 {
MarijkeZondag 8:895d941a5910 270 if(encoderA==true)
MarijkeZondag 8:895d941a5910 271 {
MarijkeZondag 8:895d941a5910 272 encoder++;
MarijkeZondag 8:895d941a5910 273 }
MarijkeZondag 8:895d941a5910 274 else
MarijkeZondag 8:895d941a5910 275 {
MarijkeZondag 8:895d941a5910 276 encoder--;
MarijkeZondag 8:895d941a5910 277 }
MarijkeZondag 8:895d941a5910 278 }
MarijkeZondag 8:895d941a5910 279
MarijkeZondag 8:895d941a5910 280 void encoderB_fall()
MarijkeZondag 8:895d941a5910 281 {
MarijkeZondag 8:895d941a5910 282 if(encoderA==false)
MarijkeZondag 8:895d941a5910 283 {
MarijkeZondag 8:895d941a5910 284 encoder++;
MarijkeZondag 8:895d941a5910 285 }
MarijkeZondag 8:895d941a5910 286 else
MarijkeZondag 8:895d941a5910 287 {
MarijkeZondag 8:895d941a5910 288 encoder--;
MarijkeZondag 8:895d941a5910 289 }
MarijkeZondag 8:895d941a5910 290 }
MarijkeZondag 8:895d941a5910 291
MarijkeZondag 8:895d941a5910 292
MarijkeZondag 9:c722418997b5 293 // Main function start.
MarijkeZondag 8:895d941a5910 294
vsluiter 0:c8f15874531b 295 int main()
MarijkeZondag 3:a3ad58758696 296 {
MarijkeZondag 13:a3d4b4daf5b4 297 //pc.baud(115200);
MarijkeZondag 13:a3d4b4daf5b4 298 //pc.printf("hello\n\r");
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 21:1da43fdbd254 304 sample_timer.attach(&sample, 0.002); //HIDscope
MarijkeZondag 21:1da43fdbd254 305
MarijkeZondag 13:a3d4b4daf5b4 306 filter_tick.attach(&emg_filtered,T); //EMG signals filtered + moving average every T sec.
MarijkeZondag 13:a3d4b4daf5b4 307 button1.rise(switch_to_calibrate); //Switch state of calibration (which muscle)
MarijkeZondag 15:be67b090b64a 308 wait(0.2f);
MarijkeZondag 13:a3d4b4daf5b4 309 button2.rise(calibrate); //calibrate threshold for 3 muscles
MarijkeZondag 15:be67b090b64a 310 wait(0.2f);
MarijkeZondag 21:1da43fdbd254 311
MarijkeZondag 13:a3d4b4daf5b4 312 pwmpin1.period_us(60); //60 microseconds PWM period, 16.7 kHz
MarijkeZondag 6:f4bbb73f3989 313
MarijkeZondag 8:895d941a5910 314 encoderA.rise(&encoderA_rise);
MarijkeZondag 8:895d941a5910 315 encoderA.fall(&encoderA_fall);
MarijkeZondag 8:895d941a5910 316 encoderB.rise(&encoderB_rise);
MarijkeZondag 8:895d941a5910 317 encoderB.fall(&encoderB_fall);
vsluiter 0:c8f15874531b 318
MarijkeZondag 16:5f7196ddc77b 319 if(emg_cal==1)
MarijkeZondag 16:5f7196ddc77b 320 {
MarijkeZondag 16:5f7196ddc77b 321 while (true)
MarijkeZondag 16:5f7196ddc77b 322 {
MarijkeZondag 16:5f7196ddc77b 323 //Motor aansturen en encoder uitlezen
MarijkeZondag 16:5f7196ddc77b 324 //float u1 = potmetervalue1;
MarijkeZondag 16:5f7196ddc77b 325 //float u2 = potmetervalue2;
MarijkeZondag 16:5f7196ddc77b 326
MarijkeZondag 16:5f7196ddc77b 327 //float m1 = ((u1*2.0f)-1.0f);
MarijkeZondag 16:5f7196ddc77b 328 //float m2 = ((u2*2.0f)-1.0f);
MarijkeZondag 16:5f7196ddc77b 329
MarijkeZondag 16:5f7196ddc77b 330 //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 331
MarijkeZondag 16:5f7196ddc77b 332 if(emgfilter0>Threshold0)
MarijkeZondag 16:5f7196ddc77b 333 {
MarijkeZondag 16:5f7196ddc77b 334 pwmpin1 = 1;
MarijkeZondag 16:5f7196ddc77b 335 directionpin1.write(1);
MarijkeZondag 16:5f7196ddc77b 336 }
MarijkeZondag 16:5f7196ddc77b 337 else
MarijkeZondag 16:5f7196ddc77b 338 {
MarijkeZondag 16:5f7196ddc77b 339 pwmpin1 = 0;
MarijkeZondag 16:5f7196ddc77b 340 }
MarijkeZondag 9:c722418997b5 341
MarijkeZondag 16:5f7196ddc77b 342 if(emgfilter1>Threshold1)
MarijkeZondag 16:5f7196ddc77b 343 {
MarijkeZondag 16:5f7196ddc77b 344 pwmpin2 = 1;
MarijkeZondag 16:5f7196ddc77b 345 directionpin2.write(1);
MarijkeZondag 16:5f7196ddc77b 346 }
MarijkeZondag 16:5f7196ddc77b 347 else
MarijkeZondag 16:5f7196ddc77b 348 {
MarijkeZondag 16:5f7196ddc77b 349 pwmpin2 = 0;
MarijkeZondag 16:5f7196ddc77b 350 }
MarijkeZondag 16:5f7196ddc77b 351 if(emgfilter2>Threshold2)
MarijkeZondag 16:5f7196ddc77b 352 {
MarijkeZondag 16:5f7196ddc77b 353 pwmpin1 = 1;
MarijkeZondag 16:5f7196ddc77b 354 pwmpin2 = 2;
MarijkeZondag 16:5f7196ddc77b 355 directionpin1.write(1);
MarijkeZondag 16:5f7196ddc77b 356 directionpin2.write(1);
MarijkeZondag 16:5f7196ddc77b 357 }
MarijkeZondag 16:5f7196ddc77b 358 else
MarijkeZondag 16:5f7196ddc77b 359 {
MarijkeZondag 16:5f7196ddc77b 360 pwmpin1 = 0;
MarijkeZondag 16:5f7196ddc77b 361 pwmpin2 = 0;
MarijkeZondag 16:5f7196ddc77b 362 }
MarijkeZondag 16:5f7196ddc77b 363
MarijkeZondag 16:5f7196ddc77b 364 //Indien waar, motor draait rechtsom. Indien niet waar, motor draait linksom.
MarijkeZondag 16:5f7196ddc77b 365 //wait(0.01f); //zodat de code niet oneindig doorgaat.
MarijkeZondag 16:5f7196ddc77b 366 //pwmpin2 = fabs(m2*0.6f)+0.4f;
MarijkeZondag 16:5f7196ddc77b 367 //directionpin2.write(m2>0);
MarijkeZondag 16:5f7196ddc77b 368
MarijkeZondag 16:5f7196ddc77b 369 //float encoderDegrees = float(encoder)*(360.0/8400.0);
MarijkeZondag 16:5f7196ddc77b 370
MarijkeZondag 16:5f7196ddc77b 371 //pc.printf("Encoder count: %f \n\r",encoderDegrees);
MarijkeZondag 10:39ec51206c8b 372
vsluiter 0:c8f15874531b 373 }
vsluiter 0:c8f15874531b 374 }
MarijkeZondag 16:5f7196ddc77b 375 }