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@28:c6b19fdfba01, 2014-10-27 (annotated)

Committer:
Tanja2211
Date:
Mon Oct 27 12:45:32 2014 +0000
Revision:
28:c6b19fdfba01
Parent:
27:9adedca4644e
Child:
29:52a0d241e1b0
motor 2

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 28:c6b19fdfba01 70 T0=filtered_emgT*100;
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 28:c6b19fdfba01 210 wait(30);
s1340735 14:bebdb2ac5ec6 211 log_timerT1.detach();
Tanja2211 15:e779bfbeb8ea 212
Tanja2211 26:e43cae7186f5 213 MOVAVG_T=MOVAVG_Positie1;
Tanja2211 15:e779bfbeb8ea 214
s1340735 10:9319e872c752 215 // positie van batje met behulp van Triceps
Tanja2211 28:c6b19fdfba01 216 drempelwaardeT=0.1;
s1340735 10:9319e872c752 217
Tanja2211 26:e43cae7186f5 218 if (MOVAVG_Positie1>= drempelwaardeT) {
s1340735 2:a86b09b00008 219 yT1=1;
s1340735 2:a86b09b00008 220 } else {
s1340735 2:a86b09b00008 221 yT1=0;
s1340735 2:a86b09b00008 222 }
Tanja2211 27:9adedca4644e 223 pc.printf("Triceps eerste meting is klaar.\n");
Tanja2211 27:9adedca4644e 224 wait(3);
Tanja2211 26:e43cae7186f5 225 //bepaling van positie met tricep 2
Tanja2211 26:e43cae7186f5 226 Ticker log_timerT2;
Tanja2211 26:e43cae7186f5 227
Tanja2211 26:e43cae7186f5 228 arm_biquad_cascade_df1_init_f32(&notchT,1,notch_const,notch_states);
Tanja2211 26:e43cae7186f5 229 arm_biquad_cascade_df1_init_f32(&lowpassT,1,lowpass_const,lowpass_states);
Tanja2211 26:e43cae7186f5 230 arm_biquad_cascade_df1_init_f32(&highpassT,1,highpass_const,highpass_states);
Tanja2211 26:e43cae7186f5 231
Tanja2211 26:e43cae7186f5 232 log_timerT2.attach(Triceps, 0.005);
Tanja2211 26:e43cae7186f5 233 wait(5);
Tanja2211 26:e43cae7186f5 234 log_timerT2.detach();
Tanja2211 26:e43cae7186f5 235
Tanja2211 26:e43cae7186f5 236 MOVAVG_T=MOVAVG_Positie2;
Tanja2211 26:e43cae7186f5 237
Tanja2211 26:e43cae7186f5 238 if (MOVAVG_Positie2 >= drempelwaardeT) {
Tanja2211 26:e43cae7186f5 239 yT2=1;
Tanja2211 26:e43cae7186f5 240 } else {
Tanja2211 26:e43cae7186f5 241 yT2=0;
Tanja2211 26:e43cae7186f5 242 }
Tanja2211 28:c6b19fdfba01 243 pc.printf("Triceps tweede meting is klaar.\n");
Tanja2211 26:e43cae7186f5 244
s1340735 10:9319e872c752 245 //*** INPUT MOTOR 2 ***
Tanja2211 26:e43cae7186f5 246 positie=yT1+yT2;
s1340735 0:abe0bc5c43b7 247
s1340735 2:a86b09b00008 248 //controle positie op scherm
s1340735 2:a86b09b00008 249 if (positie==0) {
s1340735 2:a86b09b00008 250 pc.printf("Motor 2 gaat naar stand 0\n");
Tanja2211 28:c6b19fdfba01 251 }
Tanja2211 28:c6b19fdfba01 252 if (positie==1) {
Tanja2211 28:c6b19fdfba01 253 pc.printf("Motor 2 gaat naar stand 1\n");
Tanja2211 28:c6b19fdfba01 254 }
Tanja2211 28:c6b19fdfba01 255 if (positie==2) {
Tanja2211 28:c6b19fdfba01 256 pc.printf("Motor 2 beweegt met snelheid 2\n");
Tanja2211 28:c6b19fdfba01 257
s1340735 2:a86b09b00008 258 }
s1340735 10:9319e872c752 259
s1340735 11:5044290660b0 260 wait(5);
s1340735 11:5044290660b0 261
s1340735 11:5044290660b0 262 Ticker log_timerB;
s1340735 21:f0357a41f43e 263
s1340735 19:fdebe6892633 264 arm_biquad_cascade_df1_init_f32(&notchB,1,notch_const,notch_states);
s1340735 11:5044290660b0 265 arm_biquad_cascade_df1_init_f32(&lowpassB,1,lowpass_const,lowpass_states);
s1340735 11:5044290660b0 266 arm_biquad_cascade_df1_init_f32(&highpassB,1,highpass_const,highpass_states);
s1340735 11:5044290660b0 267
s1340735 11:5044290660b0 268 log_timerB.attach(Biceps,0.005);
Tanja2211 22:0be5dfa04f75 269 wait(5);//log_timerB wordt 2000 keer uitgevoerd
s1340735 11:5044290660b0 270 log_timerB.detach();
s1340735 14:bebdb2ac5ec6 271
Tanja2211 28:c6b19fdfba01 272 //bepaling van snelheidsstand met biceps
Tanja2211 9:a1890454e5a7 273
Tanja2211 22:0be5dfa04f75 274 drempelwaardeB1=2;
Tanja2211 22:0be5dfa04f75 275 drempelwaardeB2=5;
Tanja2211 22:0be5dfa04f75 276 drempelwaardeB3=10;
s1340735 2:a86b09b00008 277
s1340735 2:a86b09b00008 278 if (MOVAVG_B >= drempelwaardeB1) {
s1340735 2:a86b09b00008 279 yB1=1;
s1340735 2:a86b09b00008 280 if (MOVAVG_B >= drempelwaardeB2) {
s1340735 2:a86b09b00008 281 yB2=1;
s1340735 2:a86b09b00008 282 if (MOVAVG_B >= drempelwaardeB3) {
s1340735 2:a86b09b00008 283 yB3=1;
s1340735 0:abe0bc5c43b7 284 } else {
s1340735 2:a86b09b00008 285 yB3=0;
s1340735 0:abe0bc5c43b7 286 }
s1340735 0:abe0bc5c43b7 287 } else {
s1340735 2:a86b09b00008 288 yB2=0;
s1340735 0:abe0bc5c43b7 289 }
s1340735 2:a86b09b00008 290 } else {
s1340735 2:a86b09b00008 291 yB1=0;
s1340735 2:a86b09b00008 292 }
s1340735 0:abe0bc5c43b7 293
Tanja2211 28:c6b19fdfba01 294 //*** INPUT MOTOR 1 ***
s1340735 10:9319e872c752 295 snelheidsstand=yB1+yB2+yB3;
s1340735 0:abe0bc5c43b7 296
Tanja2211 28:c6b19fdfba01 297 //controle snelheidsstand op scherm
s1340735 2:a86b09b00008 298 if (snelheidsstand==0) {
s1340735 2:a86b09b00008 299 pc.printf("Motor 1 beweegt niet\n");
s1340735 2:a86b09b00008 300 } else {
s1340735 1:6a8b45298e54 301 if (snelheidsstand==1) {
s1340735 1:6a8b45298e54 302 pc.printf("Motor 1 beweegt met snelheid 1\n");
s1340735 1:6a8b45298e54 303 } else {
s1340735 2:a86b09b00008 304 if (snelheidsstand==2) {
s1340735 2:a86b09b00008 305 pc.printf("Motor 1 beweegt met snelheid 2\n");
s1340735 2:a86b09b00008 306 } else {
s1340735 2:a86b09b00008 307 if (snelheidsstand==3) {
s1340735 2:a86b09b00008 308 pc.printf("Motor 1 beweegt met snelheid 3\n");
s1340735 2:a86b09b00008 309 }
s1340735 2:a86b09b00008 310 }
s1340735 1:6a8b45298e54 311 }
s1340735 1:6a8b45298e54 312 }
s1340735 10:9319e872c752 313 }