Marijke Zondag / Mbed 2 deprecated Project_script

project script

Dependencies:   HIDScope MODSERIAL biquadFilter mbed

Fork of Milestone_sample by Marijke Zondag

main.cpp@13:a3d4b4daf5b4, 2018-10-22 (annotated)

Committer:
MarijkeZondag
Date:
Mon Oct 22 08:35:21 2018 +0000
Revision:
13:a3d4b4daf5b4
Parent:
12:eaed305a76c3
Child:
14:fa09dae67390
calibration + switch_cal step included

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