Marijke Zondag / Mbed 2 deprecated VLD_direct_control

..

Dependencies:   HIDScope MODSERIAL biquadFilter mbed

main.cpp@15:be67b090b64a, 2018-10-22 (annotated)

Committer:
MarijkeZondag
Date:
Mon Oct 22 09:35:55 2018 +0000
Revision:
15:be67b090b64a
Parent:
14:fa09dae67390
Child:
16:5f7196ddc77b
zonder errors;

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