BMT M9 Groep01 / Mbed 2 deprecated test_emg

emg

Dependencies:   HIDScope MODSERIAL mbed-dsp mbed TouchButton

Fork of test by BMT M9 Groep01

main.cpp@31:01b4caea7ec0, 2014-10-27 (annotated)

Committer:
Tanja2211
Date:
Mon Oct 27 14:34:27 2014 +0000
Revision:
31:01b4caea7ec0
Parent:
30:0428978cc43e 
alles klaar

Who changed what in which revision?

UserRevisionLine numberNew contents of line
s1340735 0:abe0bc5c43b7 1 #include "mbed.h"
s1340735 0:abe0bc5c43b7 2 #include "HIDScope.h"
s1340735 0:abe0bc5c43b7 3 #include "MODSERIAL.h"
s1340735 0:abe0bc5c43b7 4 #include "arm_math.h"
s1340735 0:abe0bc5c43b7 5
s1340735 2:a86b09b00008 6 MODSERIAL pc(USBTX,USBRX);
s1340735 2:a86b09b00008 7
Tanja2211 9:a1890454e5a7 8 HIDScope scope(4);
s1340735 0:abe0bc5c43b7 9
s1340735 21:f0357a41f43e 10 AnalogIn emgB(PTB1); //biceps
s1340735 21:f0357a41f43e 11 AnalogIn emgT(PTB2); //tricep
s1340735 21:f0357a41f43e 12
s1340735 0:abe0bc5c43b7 13
s1340735 0:abe0bc5c43b7 14 //*** OBJECTS ***
s1340735 2:a86b09b00008 15 //bicep
Tanja2211 9:a1890454e5a7 16 uint16_t emg_valueB;
Tanja2211 9:a1890454e5a7 17 float emg_value_f32B;
s1340735 2:a86b09b00008 18 float filtered_emgB;
s1340735 0:abe0bc5c43b7 19 float drempelwaardeB1, drempelwaardeB2, drempelwaardeB3;//B1=snelheidsstand 1, B2=snelheidsstand 2, B3=snelheidsstand 3
s1340735 0:abe0bc5c43b7 20 int yB1, yB2, yB3;
s1340735 21:f0357a41f43e 21 float B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, B10, B11, B12, B13, B14, B15, B16, B17, B18, B19, B20, B21, B22, B23, B24, B25, B26, B27, B28, B29, MOVAVG_B; //moving average objects
s1340735 21:f0357a41f43e 22 float B30, B31, B32, B33, B34, B35, B36, B37, B38, B39, B40, B41, B42, B43, B44, B45, B46, B47, B48, B49, B50, B51, B52, B53, B54, B55, B56, B57, B58, B59;
s1340735 1:6a8b45298e54 23 int snelheidsstand;
s1340735 2:a86b09b00008 24 //tricep
Tanja2211 9:a1890454e5a7 25 uint16_t emg_valueT;
Tanja2211 9:a1890454e5a7 26 float emg_value_f32T;
s1340735 2:a86b09b00008 27 float filtered_emgT;
s1340735 14:bebdb2ac5ec6 28 float drempelwaardeT;
s1340735 2:a86b09b00008 29 int yT1, yT2;
Tanja2211 15:e779bfbeb8ea 30 float T0, T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T11, T12, T13, T14, T15, T16, T17, T18, T19, T20, T21, T22, T23, T24, T25, T26, T27, T28, T29,MOVAVG_T; //moving average objects
s1340735 14:bebdb2ac5ec6 31 float MOVAVG_Positie1, MOVAVG_Positie2;
s1340735 2:a86b09b00008 32 int positie;
s1340735 2:a86b09b00008 33
s1340735 0:abe0bc5c43b7 34
s1340735 0:abe0bc5c43b7 35 //*** FILTERS ***
s1340735 19:fdebe6892633 36 arm_biquad_casd_df1_inst_f32 notchT;
s1340735 19:fdebe6892633 37 arm_biquad_casd_df1_inst_f32 notchB;
s1340735 21:f0357a41f43e 38 //constants for 50Hz
s1340735 21:f0357a41f43e 39 float notch_const[]= {0.5857841106784856, -1.3007020142696517e-16, 0.5857841106784856, 1.3007020142696517e-16, -0.17156822135697122}; //{a0 a1 a2 -b1 -b2}
s1340735 19:fdebe6892633 40 float notch_states[4];
s1340735 19:fdebe6892633 41
Tanja2211 9:a1890454e5a7 42 arm_biquad_casd_df1_inst_f32 lowpassT;
Tanja2211 9:a1890454e5a7 43 arm_biquad_casd_df1_inst_f32 lowpassB;
s1340735 19:fdebe6892633 44 //constants for 60Hz lowpass
s1340735 19:fdebe6892633 45 float lowpass_const[] = {0.39133426347022965, 0.7826685269404593, 0.39133426347022965, -0.3695259524151476, -0.19581110146577096};//{a0 a1 a2 -b1 -b2} van online calculator
s1340735 0:abe0bc5c43b7 46 float lowpass_states[4];
s1340735 0:abe0bc5c43b7 47
Tanja2211 9:a1890454e5a7 48 arm_biquad_casd_df1_inst_f32 highpassT;
Tanja2211 9:a1890454e5a7 49 arm_biquad_casd_df1_inst_f32 highpassB;
s1340735 19:fdebe6892633 50 //constants for 20Hz highpass
Tanja2211 17:5fd768d0504f 51 float highpass_const[] = {0.6389437261127494, -1.2778874522254988, 0.6389437261127494, 1.1429772843080923, -0.41279762014290533};//{a0 a1 a2 -b1 -b2}
s1340735 0:abe0bc5c43b7 52 float highpass_states[4];
s1340735 0:abe0bc5c43b7 53
Tanja2211 9:a1890454e5a7 54
Tanja2211 9:a1890454e5a7 55 // *** TRICEPS en BICEPS EMG ***
Tanja2211 9:a1890454e5a7 56 void Triceps()
Tanja2211 9:a1890454e5a7 57 {
Tanja2211 9:a1890454e5a7 58 //Triceps lezen
Tanja2211 9:a1890454e5a7 59 emg_valueT = emgT.read_u16();
Tanja2211 9:a1890454e5a7 60 emg_value_f32T = emgT.read();
Tanja2211 9:a1890454e5a7 61
Tanja2211 9:a1890454e5a7 62 //Triceps filteren
Tanja2211 20:5cd52a0daab0 63 arm_biquad_cascade_df1_f32(&notchT, &emg_value_f32T, &filtered_emgT, 1);
s1340735 19:fdebe6892633 64 arm_biquad_cascade_df1_f32(&lowpassT, &filtered_emgT, &filtered_emgT, 1 );
Tanja2211 9:a1890454e5a7 65 filtered_emgT = fabs(filtered_emgT);
Tanja2211 17:5fd768d0504f 66 arm_biquad_cascade_df1_f32(&highpassT, &filtered_emgT, &filtered_emgT, 1 );
Tanja2211 20:5cd52a0daab0 67 filtered_emgT = fabs(filtered_emgT);
s1340735 21:f0357a41f43e 68
Tanja2211 9:a1890454e5a7 69 //Triceps moving average
Tanja2211 31:01b4caea7ec0 70 T0=filtered_emgT*1000;
Tanja2211 15:e779bfbeb8ea 71 MOVAVG_T=T0*0.03333+T1*0.03333+T2*0.03333+T3*0.03333+T4*0.03333+T5*0.03333+T6*0.03333+T7*0.03333+T8*0.03333+T9*0.03333+T10*0.03333+T11*0.03333+T12*0.03333+T13*0.03333+T14*0.03333+T15*0.03333+T16*0.03333+T17*0.03333+T18*0.03333+T19*0.03333+T20*0.03333+T21*0.03333+T22*0.03333+T23*0.03333+T24*0.03333+T25*0.03333+T26*0.03333+T27*0.03333+T28*0.03333+T29*0.03333;
Tanja2211 15:e779bfbeb8ea 72 T29=T28;
Tanja2211 15:e779bfbeb8ea 73 T28=T27;
Tanja2211 15:e779bfbeb8ea 74 T27=T26;
Tanja2211 15:e779bfbeb8ea 75 T26=T25;
Tanja2211 15:e779bfbeb8ea 76 T25=T24;
Tanja2211 15:e779bfbeb8ea 77 T24=T23;
Tanja2211 15:e779bfbeb8ea 78 T23=T22;
Tanja2211 15:e779bfbeb8ea 79 T22=T21;
Tanja2211 15:e779bfbeb8ea 80 T21=T20;
s1340735 21:f0357a41f43e 81 T20=T19;
Tanja2211 15:e779bfbeb8ea 82 T19=T18;
Tanja2211 15:e779bfbeb8ea 83 T18=T17;
Tanja2211 15:e779bfbeb8ea 84 T17=T16;
Tanja2211 15:e779bfbeb8ea 85 T16=T15;
Tanja2211 15:e779bfbeb8ea 86 T15=T14;
Tanja2211 15:e779bfbeb8ea 87 T14=T13;
Tanja2211 15:e779bfbeb8ea 88 T13=T12;
Tanja2211 15:e779bfbeb8ea 89 T12=T11;
Tanja2211 15:e779bfbeb8ea 90 T11=T10;
Tanja2211 15:e779bfbeb8ea 91 T10=T9;
Tanja2211 9:a1890454e5a7 92 T9=T8;
Tanja2211 9:a1890454e5a7 93 T8=T7;
Tanja2211 9:a1890454e5a7 94 T7=T6;
Tanja2211 9:a1890454e5a7 95 T6=T5;
Tanja2211 9:a1890454e5a7 96 T5=T4;
Tanja2211 9:a1890454e5a7 97 T4=T3;
Tanja2211 9:a1890454e5a7 98 T3=T2;
Tanja2211 9:a1890454e5a7 99 T2=T1;
Tanja2211 9:a1890454e5a7 100 T1=T0;
s1340735 21:f0357a41f43e 101
Tanja2211 16:e997a6fd802a 102 //sturen naar scherm (Realterm)
Tanja2211 16:e997a6fd802a 103 pc.printf("%f\r\n",MOVAVG_T);
Tanja2211 16:e997a6fd802a 104
s1340735 10:9319e872c752 105 //sturen naar HID Scope
Tanja2211 9:a1890454e5a7 106 scope.set(0,emg_valueT); //ruwe data
Tanja2211 9:a1890454e5a7 107 scope.set(1,filtered_emgT); //filtered
Tanja2211 9:a1890454e5a7 108 scope.send();
Tanja2211 9:a1890454e5a7 109 }
Tanja2211 9:a1890454e5a7 110
s1340735 0:abe0bc5c43b7 111 void Biceps()
s1340735 0:abe0bc5c43b7 112 {
Tanja2211 9:a1890454e5a7 113 //Biceps lezen
s1340735 0:abe0bc5c43b7 114 emg_valueB = emgB.read_u16(); // read direct ADC result, converted to 16 bit integer (0..2^16 = 0..65536 = 0..3.3V)
s1340735 0:abe0bc5c43b7 115 emg_value_f32B = emgB.read();
s1340735 0:abe0bc5c43b7 116
Tanja2211 9:a1890454e5a7 117 //Biceps filteren
s1340735 19:fdebe6892633 118 arm_biquad_cascade_df1_f32(&notchB, &emg_value_f32B, &filtered_emgB, 1 );
s1340735 19:fdebe6892633 119 arm_biquad_cascade_df1_f32(&lowpassB, &filtered_emgB, &filtered_emgB, 1 );
s1340735 0:abe0bc5c43b7 120 filtered_emgB = fabs(filtered_emgB);
Tanja2211 17:5fd768d0504f 121 arm_biquad_cascade_df1_f32(&highpassB, &filtered_emgB, &filtered_emgB, 1 );
Tanja2211 20:5cd52a0daab0 122 filtered_emgB = fabs(filtered_emgB);
s1340735 21:f0357a41f43e 123
Tanja2211 9:a1890454e5a7 124 //Biceps moving average
Tanja2211 22:0be5dfa04f75 125 B0=filtered_emgB*1000;
s1340735 21:f0357a41f43e 126 MOVAVG_B=B0*0.01667+B1*0.01667+B2*0.01667+B3*0.01667+B4*0.01667+B5*0.01667+B6*0.01667+B7*0.01667+B8*0.01667+B9*0.01667+B10*0.01667+B11*0.01667+B12*0.01667+B13*0.01667+B14*0.01667+B15*0.01667+B16*0.01667+B17*0.01667+B18*0.01667+B19*0.01667+B20*0.01667+B21*0.01667+B22*0.01667+B23*0.01667+B24*0.01667+B25*0.01667+B26*0.01667+B27*0.01667+B28*0.01667+B29*0.01667+B30*0.01667+B31*0.01667+B32*0.01667+B33*0.01667+B34*0.01667+B35*0.01667+B36*0.01667+B37*0.01667+B38*0.01667+B39*0.01667+B40*0.01667+B41*0.01667+B42*0.01667+B43*0.01667+B44*0.01667+B45*0.01667+B46*0.01667+B47*0.01667+B48*0.01667+B49*0.01667+B50*0.01667+B51*0.01667+B52*0.01667+B53*0.01667+B54*0.01667+B55*0.01667+B56*0.01667+B57*0.01667+B58*0.01667+B59*0.01667;
s1340735 21:f0357a41f43e 127 B59=B58;
s1340735 21:f0357a41f43e 128 B58=B57;
s1340735 21:f0357a41f43e 129 B57=B56;
s1340735 21:f0357a41f43e 130 B56=B55;
s1340735 21:f0357a41f43e 131 B55=B54;
s1340735 21:f0357a41f43e 132 B54=B53;
s1340735 21:f0357a41f43e 133 B53=B52;
s1340735 21:f0357a41f43e 134 B52=B51;
s1340735 21:f0357a41f43e 135 B51=B50;
s1340735 21:f0357a41f43e 136 B50=B48;
s1340735 21:f0357a41f43e 137 B49=B49;
s1340735 21:f0357a41f43e 138 B48=B47;
s1340735 21:f0357a41f43e 139 B47=B46;
s1340735 21:f0357a41f43e 140 B46=B45;
s1340735 21:f0357a41f43e 141 B45=B44;
s1340735 21:f0357a41f43e 142 B44=B43;
s1340735 21:f0357a41f43e 143 B43=B42;
s1340735 21:f0357a41f43e 144 B42=B41;
s1340735 21:f0357a41f43e 145 B41=B40;
s1340735 21:f0357a41f43e 146 B40=B39;
s1340735 21:f0357a41f43e 147 B39=B38;
s1340735 21:f0357a41f43e 148 B38=B37;
s1340735 21:f0357a41f43e 149 B37=B36;
s1340735 21:f0357a41f43e 150 B36=B35;
s1340735 21:f0357a41f43e 151 B35=B34;
s1340735 21:f0357a41f43e 152 B34=B33;
s1340735 21:f0357a41f43e 153 B33=B32;
s1340735 21:f0357a41f43e 154 B32=B31;
s1340735 21:f0357a41f43e 155 B31=B30;
s1340735 21:f0357a41f43e 156 B30=B29;
Tanja2211 15:e779bfbeb8ea 157 B29=B28;
Tanja2211 15:e779bfbeb8ea 158 B28=B27;
Tanja2211 15:e779bfbeb8ea 159 B27=B26;
Tanja2211 15:e779bfbeb8ea 160 B26=B25;
Tanja2211 15:e779bfbeb8ea 161 B25=B24;
Tanja2211 15:e779bfbeb8ea 162 B24=B23;
Tanja2211 15:e779bfbeb8ea 163 B23=B22;
Tanja2211 15:e779bfbeb8ea 164 B22=B21;
Tanja2211 15:e779bfbeb8ea 165 B21=B20;
s1340735 21:f0357a41f43e 166 B20=B19;
Tanja2211 15:e779bfbeb8ea 167 B19=B18;
Tanja2211 15:e779bfbeb8ea 168 B18=B17;
Tanja2211 15:e779bfbeb8ea 169 B17=B16;
Tanja2211 15:e779bfbeb8ea 170 B16=B15;
Tanja2211 15:e779bfbeb8ea 171 B15=B14;
Tanja2211 15:e779bfbeb8ea 172 B14=B13;
Tanja2211 15:e779bfbeb8ea 173 B13=B12;
Tanja2211 15:e779bfbeb8ea 174 B12=B11;
Tanja2211 15:e779bfbeb8ea 175 B11=B10;
Tanja2211 15:e779bfbeb8ea 176 B10=B9;
s1340735 0:abe0bc5c43b7 177 B9=B8;
s1340735 0:abe0bc5c43b7 178 B8=B7;
s1340735 0:abe0bc5c43b7 179 B7=B6;
s1340735 0:abe0bc5c43b7 180 B6=B5;
s1340735 0:abe0bc5c43b7 181 B5=B4;
s1340735 0:abe0bc5c43b7 182 B4=B3;
s1340735 0:abe0bc5c43b7 183 B3=B2;
s1340735 0:abe0bc5c43b7 184 B2=B1;
s1340735 0:abe0bc5c43b7 185 B1=B0;
s1340735 0:abe0bc5c43b7 186
Tanja2211 16:e997a6fd802a 187 //sturen naar scherm
Tanja2211 16:e997a6fd802a 188 pc.printf("%f\r\n",MOVAVG_B);
Tanja2211 16:e997a6fd802a 189
s1340735 10:9319e872c752 190 //naar HID Scope
Tanja2211 6:ad6d4bd69205 191 scope.set(2,emg_valueB); //ruwe data
Tanja2211 6:ad6d4bd69205 192 scope.set(3,filtered_emgB); //filtered
Tanja2211 5:3232d78cb7b3 193 scope.send();
s1340735 2:a86b09b00008 194 }
s1340735 0:abe0bc5c43b7 195
s1340735 19:fdebe6892633 196
s1340735 0:abe0bc5c43b7 197 // *** MAIN ***
s1340735 2:a86b09b00008 198 int main()
Tanja2211 9:a1890454e5a7 199 {
Tanja2211 9:a1890454e5a7 200 pc.baud(115200);
s1340735 10:9319e872c752 201
s1340735 14:bebdb2ac5ec6 202 //bepaling van positie met triceps 1
s1340735 14:bebdb2ac5ec6 203 Ticker log_timerT1;
s1340735 21:f0357a41f43e 204
s1340735 19:fdebe6892633 205 arm_biquad_cascade_df1_init_f32(&notchT,1,notch_const,notch_states);
Tanja2211 9:a1890454e5a7 206 arm_biquad_cascade_df1_init_f32(&lowpassT,1,lowpass_const,lowpass_states);
Tanja2211 9:a1890454e5a7 207 arm_biquad_cascade_df1_init_f32(&highpassT,1,highpass_const,highpass_states);
s1340735 2:a86b09b00008 208
s1340735 14:bebdb2ac5ec6 209 log_timerT1.attach(Triceps, 0.005);
Tanja2211 31:01b4caea7ec0 210 wait(3);
s1340735 14:bebdb2ac5ec6 211 log_timerT1.detach();
Tanja2211 15:e779bfbeb8ea 212
Tanja2211 30:0428978cc43e 213 //MOVAVG_T=MOVAVG_Positie1;
Tanja2211 15:e779bfbeb8ea 214
s1340735 10:9319e872c752 215 // positie van batje met behulp van Triceps
Tanja2211 31:01b4caea7ec0 216 drempelwaardeT=2.5;
s1340735 10:9319e872c752 217
Tanja2211 30:0428978cc43e 218 if (MOVAVG_T>= drempelwaardeT) {
s1340735 2:a86b09b00008 219 yT1=1;
s1340735 2:a86b09b00008 220 } else {
s1340735 2:a86b09b00008 221 yT1=0;
s1340735 2:a86b09b00008 222 }
s1340735 29:52a0d241e1b0 223
Tanja2211 27:9adedca4644e 224 pc.printf("Triceps eerste meting is klaar.\n");
Tanja2211 31:01b4caea7ec0 225 wait(2);
s1340735 29:52a0d241e1b0 226
Tanja2211 26:e43cae7186f5 227 //bepaling van positie met tricep 2
Tanja2211 26:e43cae7186f5 228 Ticker log_timerT2;
Tanja2211 26:e43cae7186f5 229
Tanja2211 26:e43cae7186f5 230 arm_biquad_cascade_df1_init_f32(&notchT,1,notch_const,notch_states);
Tanja2211 26:e43cae7186f5 231 arm_biquad_cascade_df1_init_f32(&lowpassT,1,lowpass_const,lowpass_states);
Tanja2211 26:e43cae7186f5 232 arm_biquad_cascade_df1_init_f32(&highpassT,1,highpass_const,highpass_states);
Tanja2211 26:e43cae7186f5 233
Tanja2211 26:e43cae7186f5 234 log_timerT2.attach(Triceps, 0.005);
Tanja2211 31:01b4caea7ec0 235 wait(3);
Tanja2211 26:e43cae7186f5 236 log_timerT2.detach();
Tanja2211 26:e43cae7186f5 237
Tanja2211 30:0428978cc43e 238 //MOVAVG_T=MOVAVG_Positie2;
Tanja2211 26:e43cae7186f5 239
Tanja2211 30:0428978cc43e 240 if (MOVAVG_T >= drempelwaardeT) {
Tanja2211 26:e43cae7186f5 241 yT2=1;
Tanja2211 26:e43cae7186f5 242 } else {
Tanja2211 26:e43cae7186f5 243 yT2=0;
Tanja2211 26:e43cae7186f5 244 }
s1340735 29:52a0d241e1b0 245
Tanja2211 28:c6b19fdfba01 246 pc.printf("Triceps tweede meting is klaar.\n");
Tanja2211 26:e43cae7186f5 247
s1340735 10:9319e872c752 248 //*** INPUT MOTOR 2 ***
Tanja2211 26:e43cae7186f5 249 positie=yT1+yT2;
s1340735 0:abe0bc5c43b7 250
s1340735 2:a86b09b00008 251 //controle positie op scherm
s1340735 2:a86b09b00008 252 if (positie==0) {
Tanja2211 30:0428978cc43e 253 pc.printf("Motor 2 blijft op stand 0\n");
s1340735 29:52a0d241e1b0 254 } else {
s1340735 29:52a0d241e1b0 255 if (positie==1) {
Tanja2211 30:0428978cc43e 256 pc.printf("Motor 2 gaat naar stand 1\n");
s1340735 29:52a0d241e1b0 257 } else {
s1340735 29:52a0d241e1b0 258 if (positie==2) {
Tanja2211 30:0428978cc43e 259 pc.printf("Motor 2 gaat naar stand 2\n");
s1340735 29:52a0d241e1b0 260 }
s1340735 29:52a0d241e1b0 261 }
s1340735 2:a86b09b00008 262 }
s1340735 10:9319e872c752 263
Tanja2211 31:01b4caea7ec0 264 wait(2);
s1340735 11:5044290660b0 265
s1340735 11:5044290660b0 266 Ticker log_timerB;
s1340735 21:f0357a41f43e 267
s1340735 19:fdebe6892633 268 arm_biquad_cascade_df1_init_f32(&notchB,1,notch_const,notch_states);
s1340735 11:5044290660b0 269 arm_biquad_cascade_df1_init_f32(&lowpassB,1,lowpass_const,lowpass_states);
s1340735 11:5044290660b0 270 arm_biquad_cascade_df1_init_f32(&highpassB,1,highpass_const,highpass_states);
s1340735 11:5044290660b0 271
s1340735 11:5044290660b0 272 log_timerB.attach(Biceps,0.005);
Tanja2211 31:01b4caea7ec0 273 wait(3);
s1340735 11:5044290660b0 274 log_timerB.detach();
s1340735 14:bebdb2ac5ec6 275
Tanja2211 28:c6b19fdfba01 276 //bepaling van snelheidsstand met biceps
Tanja2211 22:0be5dfa04f75 277 drempelwaardeB1=2;
Tanja2211 22:0be5dfa04f75 278 drempelwaardeB2=5;
Tanja2211 22:0be5dfa04f75 279 drempelwaardeB3=10;
s1340735 2:a86b09b00008 280
s1340735 2:a86b09b00008 281 if (MOVAVG_B >= drempelwaardeB1) {
s1340735 2:a86b09b00008 282 yB1=1;
s1340735 2:a86b09b00008 283 if (MOVAVG_B >= drempelwaardeB2) {
s1340735 2:a86b09b00008 284 yB2=1;
s1340735 2:a86b09b00008 285 if (MOVAVG_B >= drempelwaardeB3) {
s1340735 2:a86b09b00008 286 yB3=1;
s1340735 0:abe0bc5c43b7 287 } else {
s1340735 2:a86b09b00008 288 yB3=0;
s1340735 0:abe0bc5c43b7 289 }
s1340735 0:abe0bc5c43b7 290 } else {
s1340735 2:a86b09b00008 291 yB2=0;
s1340735 0:abe0bc5c43b7 292 }
s1340735 2:a86b09b00008 293 } else {
s1340735 2:a86b09b00008 294 yB1=0;
s1340735 2:a86b09b00008 295 }
s1340735 0:abe0bc5c43b7 296
Tanja2211 28:c6b19fdfba01 297 //*** INPUT MOTOR 1 ***
s1340735 10:9319e872c752 298 snelheidsstand=yB1+yB2+yB3;
s1340735 0:abe0bc5c43b7 299
Tanja2211 28:c6b19fdfba01 300 //controle snelheidsstand op scherm
s1340735 2:a86b09b00008 301 if (snelheidsstand==0) {
s1340735 2:a86b09b00008 302 pc.printf("Motor 1 beweegt niet\n");
s1340735 2:a86b09b00008 303 } else {
s1340735 1:6a8b45298e54 304 if (snelheidsstand==1) {
s1340735 1:6a8b45298e54 305 pc.printf("Motor 1 beweegt met snelheid 1\n");
s1340735 1:6a8b45298e54 306 } else {
s1340735 2:a86b09b00008 307 if (snelheidsstand==2) {
s1340735 2:a86b09b00008 308 pc.printf("Motor 1 beweegt met snelheid 2\n");
s1340735 2:a86b09b00008 309 } else {
s1340735 2:a86b09b00008 310 if (snelheidsstand==3) {
s1340735 2:a86b09b00008 311 pc.printf("Motor 1 beweegt met snelheid 3\n");
s1340735 2:a86b09b00008 312 }
s1340735 2:a86b09b00008 313 }
s1340735 1:6a8b45298e54 314 }
s1340735 1:6a8b45298e54 315 }
s1340735 10:9319e872c752 316 }