project enzo / Mbed 2 deprecated robot Featured

dit is em

Dependencies:   mbed

main.cpp@11:aac90fca0290, 2017-05-09 (annotated)

Committer:
bjorntukkertje
Date:
Tue May 09 13:59:06 2017 +0000
Revision:
11:aac90fca0290
Parent:
10:18116d979d61
Child:
12:2f3afa8d0ddc
met zoek sensor;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bjorntukkertje 0:9756a10feb5f 1 #include "mbed.h"
bjorntukkertje 0:9756a10feb5f 2
bjorntukkertje 11:aac90fca0290 3 PwmOut MotorL1 (D13);
bjorntukkertje 11:aac90fca0290 4 PwmOut MotorL2 (A0);
bjorntukkertje 11:aac90fca0290 5 PwmOut MotorR3 (D12);
bjorntukkertje 11:aac90fca0290 6 PwmOut MotorR4 (D11);
bjorntukkertje 6:6659a8ada914 7 AnalogIn Sensor1 (A1);
bjorntukkertje 6:6659a8ada914 8 AnalogIn Sensor2 (A2);
bjorntukkertje 6:6659a8ada914 9 AnalogIn Sensor3 (A3);
bjorntukkertje 6:6659a8ada914 10 AnalogIn Sensor4 (A4);
bjorntukkertje 11:aac90fca0290 11 AnalogIn zoeksensor (A5);
bjorntukkertje 10:18116d979d61 12 DigitalOut steppera (D2);
bjorntukkertje 10:18116d979d61 13 DigitalOut stepperb (D3);
bjorntukkertje 10:18116d979d61 14 DigitalOut stepperc (D4);
bjorntukkertje 10:18116d979d61 15 DigitalOut stepperd (D5);
bjorntukkertje 4:038346651cc0 16
bjorntukkertje 0:9756a10feb5f 17
bjorntukkertje 10:18116d979d61 18 Ticker lidar;
bjorntukkertje 10:18116d979d61 19 Ticker serial;
bjorntukkertje 10:18116d979d61 20
bjorntukkertje 10:18116d979d61 21 Serial pc(USBTX, USBRX);
bjorntukkertje 0:9756a10feb5f 22
bjorntukkertje 11:aac90fca0290 23 int afstand = zoeksensor;
bjorntukkertje 11:aac90fca0290 24
bjorntukkertje 10:18116d979d61 25 int stapmode = 0;
bjorntukkertje 10:18116d979d61 26 int position = 0;
bjorntukkertje 10:18116d979d61 27 int pos;
bjorntukkertje 10:18116d979d61 28
bjorntukkertje 10:18116d979d61 29 // poar neemn
bjorntukkertje 10:18116d979d61 30 // twee poar neemn
bjorntukkertje 10:18116d979d61 31 // twee tettn in n envelop
bjorntukkertje 10:18116d979d61 32
bjorntukkertje 10:18116d979d61 33 int stepper(int stapmode)
bjorntukkertje 10:18116d979d61 34 {
bjorntukkertje 10:18116d979d61 35 switch (stapmode) {
bjorntukkertje 10:18116d979d61 36 case 0:
bjorntukkertje 10:18116d979d61 37 steppera = 1;
bjorntukkertje 10:18116d979d61 38 stepperb = 0;
bjorntukkertje 10:18116d979d61 39 stepperc = 1;
bjorntukkertje 10:18116d979d61 40 stepperd = 0;
bjorntukkertje 10:18116d979d61 41 pos ++;
bjorntukkertje 10:18116d979d61 42 break;
bjorntukkertje 10:18116d979d61 43 case 1:
bjorntukkertje 10:18116d979d61 44 steppera = 1;
bjorntukkertje 10:18116d979d61 45 stepperb = 0;
bjorntukkertje 10:18116d979d61 46 stepperc = 0;
bjorntukkertje 10:18116d979d61 47 stepperd = 1;
bjorntukkertje 10:18116d979d61 48 pos ++;
bjorntukkertje 10:18116d979d61 49 break;
bjorntukkertje 10:18116d979d61 50 case 2:
bjorntukkertje 10:18116d979d61 51 steppera = 0;
bjorntukkertje 10:18116d979d61 52 stepperb = 1;
bjorntukkertje 10:18116d979d61 53 stepperc = 0;
bjorntukkertje 10:18116d979d61 54 stepperd = 1;
bjorntukkertje 10:18116d979d61 55 pos ++;
bjorntukkertje 10:18116d979d61 56 break;
bjorntukkertje 10:18116d979d61 57 case 3:
bjorntukkertje 10:18116d979d61 58 steppera = 0;
bjorntukkertje 10:18116d979d61 59 stepperb = 1;
bjorntukkertje 10:18116d979d61 60 stepperc = 1;
bjorntukkertje 10:18116d979d61 61 stepperd = 0;
bjorntukkertje 10:18116d979d61 62 pos ++;
bjorntukkertje 10:18116d979d61 63 break;
bjorntukkertje 10:18116d979d61 64 ;
bjorntukkertje 10:18116d979d61 65 }
bjorntukkertje 10:18116d979d61 66 if (pos > 1000) { //na volledige rotatie ga naar nul
bjorntukkertje 10:18116d979d61 67 pos = 0;
bjorntukkertje 10:18116d979d61 68 }
bjorntukkertje 10:18116d979d61 69 return pos;
bjorntukkertje 10:18116d979d61 70
bjorntukkertje 10:18116d979d61 71 }
bjorntukkertje 10:18116d979d61 72
bjorntukkertje 10:18116d979d61 73 void run_serial() //deze functie word periodiek aangeroepen om de stappenmotor te draaien en de afstand te meten
bjorntukkertje 10:18116d979d61 74 {
bjorntukkertje 10:18116d979d61 75
bjorntukkertje 10:18116d979d61 76 pc.printf("%d\n",position);
bjorntukkertje 10:18116d979d61 77 }
bjorntukkertje 10:18116d979d61 78
bjorntukkertje 10:18116d979d61 79 void run_lidar() //deze functie word periodiek aangeroepen om de stappenmotor te draaien en de afstand te meten
bjorntukkertje 10:18116d979d61 80 {
bjorntukkertje 10:18116d979d61 81 stapmode ++;
bjorntukkertje 10:18116d979d61 82 if (stapmode > 3) {
bjorntukkertje 10:18116d979d61 83 stapmode = 0;
bjorntukkertje 10:18116d979d61 84 }
bjorntukkertje 10:18116d979d61 85 position = stepper(stapmode);
bjorntukkertje 10:18116d979d61 86 }
bjorntukkertje 10:18116d979d61 87
bjorntukkertje 8:ec7b7c9ad07e 88
bjorntukkertje 0:9756a10feb5f 89 int main() {
bjorntukkertje 10:18116d979d61 90 lidar.attach(&run_lidar, 0.0001);
bjorntukkertje 10:18116d979d61 91 serial.attach(&run_serial, 0.2);
bjorntukkertje 0:9756a10feb5f 92 while(1) {
bjorntukkertje 8:ec7b7c9ad07e 93 if ( Sensor1 >= 1) {
bjorntukkertje 11:aac90fca0290 94 MotorL1 = 0 ;
bjorntukkertje 11:aac90fca0290 95 MotorL2 = 1 ;
bjorntukkertje 11:aac90fca0290 96 MotorR3 = 0 ;
bjorntukkertje 11:aac90fca0290 97 MotorR4 = 1 ;
bjorntukkertje 8:ec7b7c9ad07e 98 wait_ms (500);
bjorntukkertje 11:aac90fca0290 99 MotorL1 = 1 ;
bjorntukkertje 11:aac90fca0290 100 MotorL2 = 0 ;
bjorntukkertje 11:aac90fca0290 101 MotorR3 = 0 ;
bjorntukkertje 11:aac90fca0290 102 MotorR4 = 1 ;
bjorntukkertje 8:ec7b7c9ad07e 103 wait_ms (1000);
bjorntukkertje 8:ec7b7c9ad07e 104 }
bjorntukkertje 8:ec7b7c9ad07e 105 if (Sensor2 >= 1) {
bjorntukkertje 11:aac90fca0290 106 MotorL1 = 0 ;
bjorntukkertje 11:aac90fca0290 107 MotorL2 = 1 ;
bjorntukkertje 11:aac90fca0290 108 MotorR3 = 0 ;
bjorntukkertje 11:aac90fca0290 109 MotorR4 = 1 ;
bjorntukkertje 6:6659a8ada914 110 wait_ms (500);
bjorntukkertje 11:aac90fca0290 111 MotorL1 = 1 ;
bjorntukkertje 11:aac90fca0290 112 MotorL2 = 0 ;
bjorntukkertje 11:aac90fca0290 113 MotorR3 = 0 ;
bjorntukkertje 11:aac90fca0290 114 MotorR4 = 1 ;
bjorntukkertje 6:6659a8ada914 115 wait_ms (1000);
bjorntukkertje 6:6659a8ada914 116 }
bjorntukkertje 8:ec7b7c9ad07e 117 if (Sensor3 >= 1) {
bjorntukkertje 11:aac90fca0290 118 MotorL1 = 0 ;
bjorntukkertje 11:aac90fca0290 119 MotorL2 = 1 ;
bjorntukkertje 11:aac90fca0290 120 MotorR3 = 0 ;
bjorntukkertje 11:aac90fca0290 121 MotorR4 = 1 ;
bjorntukkertje 8:ec7b7c9ad07e 122 wait_ms (500);
bjorntukkertje 11:aac90fca0290 123 MotorL1 = 1 ;
bjorntukkertje 11:aac90fca0290 124 MotorL2 = 0 ;
bjorntukkertje 11:aac90fca0290 125 MotorR3 = 0 ;
bjorntukkertje 11:aac90fca0290 126 MotorR4 = 1 ;
bjorntukkertje 8:ec7b7c9ad07e 127 wait_ms (1000);
bjorntukkertje 8:ec7b7c9ad07e 128 }
bjorntukkertje 8:ec7b7c9ad07e 129 if (Sensor4 >= 1) {
bjorntukkertje 11:aac90fca0290 130 MotorL1 = 0 ;
bjorntukkertje 11:aac90fca0290 131 MotorL2 = 1 ;
bjorntukkertje 11:aac90fca0290 132 MotorR3 = 0 ;
bjorntukkertje 11:aac90fca0290 133 MotorR4 = 1 ;
bjorntukkertje 8:ec7b7c9ad07e 134 wait_ms (500);
bjorntukkertje 11:aac90fca0290 135 MotorL1 = 1 ;
bjorntukkertje 11:aac90fca0290 136 MotorL2 = 0 ;
bjorntukkertje 11:aac90fca0290 137 MotorR3 = 0 ;
bjorntukkertje 11:aac90fca0290 138 MotorR4 = 1 ;
bjorntukkertje 8:ec7b7c9ad07e 139 wait_ms (1000);
bjorntukkertje 8:ec7b7c9ad07e 140 }
bjorntukkertje 2:5c11199e4ff2 141 else {
bjorntukkertje 11:aac90fca0290 142 MotorL1 = 1 ;
bjorntukkertje 11:aac90fca0290 143 MotorL2 = 0 ;
bjorntukkertje 11:aac90fca0290 144 MotorR3 = 1 ;
bjorntukkertje 11:aac90fca0290 145 MotorR4 = 0 ;
bjorntukkertje 2:5c11199e4ff2 146 }
bjorntukkertje 11:aac90fca0290 147
bjorntukkertje 0:9756a10feb5f 148
bjorntukkertje 0:9756a10feb5f 149 }
bjorntukkertje 11:aac90fca0290 150 while (1){
bjorntukkertje 11:aac90fca0290 151
bjorntukkertje 11:aac90fca0290 152 if (afstand > 0.25 && pos > 17 && pos < 45) {
bjorntukkertje 11:aac90fca0290 153 // 10 graden
bjorntukkertje 11:aac90fca0290 154 MotorL1 = 1 ;
bjorntukkertje 11:aac90fca0290 155 MotorL2 = 0 ;
bjorntukkertje 11:aac90fca0290 156 MotorR3 = 0.95 ;
bjorntukkertje 11:aac90fca0290 157 MotorR4 = 0 ;
bjorntukkertje 11:aac90fca0290 158 wait_ms (1000);
bjorntukkertje 11:aac90fca0290 159 }
bjorntukkertje 11:aac90fca0290 160 if (afstand > 0.25 && pos >= 45&& pos < 72) {
bjorntukkertje 11:aac90fca0290 161 // 20 graden
bjorntukkertje 11:aac90fca0290 162 MotorL1 = 1 ;
bjorntukkertje 11:aac90fca0290 163 MotorL2 = 0 ;
bjorntukkertje 11:aac90fca0290 164 MotorR3 = 0.9 ;
bjorntukkertje 11:aac90fca0290 165 MotorR4 = 0 ;
bjorntukkertje 11:aac90fca0290 166 wait_ms (1000);
bjorntukkertje 11:aac90fca0290 167 }
bjorntukkertje 11:aac90fca0290 168 if (afstand > 0.25 && pos >= 72 && pos < 100) {
bjorntukkertje 11:aac90fca0290 169 // 30 graden
bjorntukkertje 11:aac90fca0290 170 MotorL1 = 1 ;
bjorntukkertje 11:aac90fca0290 171 MotorL2 = 0 ;
bjorntukkertje 11:aac90fca0290 172 MotorR3 = 0.85 ;
bjorntukkertje 11:aac90fca0290 173 MotorR4 = 0 ;
bjorntukkertje 11:aac90fca0290 174 wait_ms (1000);
bjorntukkertje 11:aac90fca0290 175 }
bjorntukkertje 11:aac90fca0290 176 if (afstand > 0.25 && pos >= 100 && pos < 128) {
bjorntukkertje 11:aac90fca0290 177 // 40 graden
bjorntukkertje 11:aac90fca0290 178 MotorL1 = 1 ;
bjorntukkertje 11:aac90fca0290 179 MotorL2 = 0 ;
bjorntukkertje 11:aac90fca0290 180 MotorR3 = 0.8 ;
bjorntukkertje 11:aac90fca0290 181 MotorR4 = 0 ;
bjorntukkertje 11:aac90fca0290 182 wait_ms (1000);
bjorntukkertje 11:aac90fca0290 183 }
bjorntukkertje 11:aac90fca0290 184 if (afstand > 0.25 && pos >= 128 && pos < 156) {
bjorntukkertje 11:aac90fca0290 185 // 50 graden
bjorntukkertje 11:aac90fca0290 186 MotorL1 = 1 ;
bjorntukkertje 11:aac90fca0290 187 MotorL2 = 0 ;
bjorntukkertje 11:aac90fca0290 188 MotorR3 = 0.75 ;
bjorntukkertje 11:aac90fca0290 189 MotorR4 = 0 ;
bjorntukkertje 11:aac90fca0290 190 wait_ms (1000);
bjorntukkertje 11:aac90fca0290 191 }
bjorntukkertje 11:aac90fca0290 192 if (afstand > 0.25 && pos >= 156 && pos < 183) {
bjorntukkertje 11:aac90fca0290 193 // 60 graden
bjorntukkertje 11:aac90fca0290 194 MotorL1 = 1 ;
bjorntukkertje 11:aac90fca0290 195 MotorL2 = 0 ;
bjorntukkertje 11:aac90fca0290 196 MotorR3 = 0.7 ;
bjorntukkertje 11:aac90fca0290 197 MotorR4 = 0 ;
bjorntukkertje 11:aac90fca0290 198 wait_ms (1000);
bjorntukkertje 11:aac90fca0290 199 }
bjorntukkertje 11:aac90fca0290 200 if (afstand > 0.25 && pos >= 183 && pos < 211) {
bjorntukkertje 11:aac90fca0290 201 // 70 graden
bjorntukkertje 11:aac90fca0290 202 MotorL1 = 1 ;
bjorntukkertje 11:aac90fca0290 203 MotorL2 = 0 ;
bjorntukkertje 11:aac90fca0290 204 MotorR3 = 0.65 ;
bjorntukkertje 11:aac90fca0290 205 MotorR4 = 0 ;
bjorntukkertje 11:aac90fca0290 206 wait_ms (1000);
bjorntukkertje 11:aac90fca0290 207 }
bjorntukkertje 11:aac90fca0290 208 if (afstand > 0.25 && pos >= 211 && pos < 239) {
bjorntukkertje 11:aac90fca0290 209 // 80 graden
bjorntukkertje 11:aac90fca0290 210 MotorL1 = 1 ;
bjorntukkertje 11:aac90fca0290 211 MotorL2 = 0 ;
bjorntukkertje 11:aac90fca0290 212 MotorR3 = 0.6 ;
bjorntukkertje 11:aac90fca0290 213 MotorR4 = 0 ;
bjorntukkertje 11:aac90fca0290 214 wait_ms (1000);
bjorntukkertje 11:aac90fca0290 215 }
bjorntukkertje 11:aac90fca0290 216 if (afstand > 0.25 && pos >= 239 && pos < 266) {
bjorntukkertje 11:aac90fca0290 217 // 90 graden
bjorntukkertje 11:aac90fca0290 218 MotorL1 = 1 ;
bjorntukkertje 11:aac90fca0290 219 MotorL2 = 0 ;
bjorntukkertje 11:aac90fca0290 220 MotorR3 = 0.55 ;
bjorntukkertje 11:aac90fca0290 221 MotorR4 = 0 ;
bjorntukkertje 11:aac90fca0290 222 wait_ms (1000);
bjorntukkertje 11:aac90fca0290 223 }
bjorntukkertje 11:aac90fca0290 224 if (afstand > 0.25 && pos >= 266 && pos < 294) {
bjorntukkertje 11:aac90fca0290 225 // 100 graden
bjorntukkertje 11:aac90fca0290 226 MotorL1 = 1 ;
bjorntukkertje 11:aac90fca0290 227 MotorL2 = 0 ;
bjorntukkertje 11:aac90fca0290 228 MotorR3 = 0.5 ;
bjorntukkertje 11:aac90fca0290 229 MotorR4 = 0 ;
bjorntukkertje 11:aac90fca0290 230 wait_ms (1000);
bjorntukkertje 11:aac90fca0290 231 }
bjorntukkertje 11:aac90fca0290 232 if (afstand > 0.25 && pos >= 294 && pos < 322) {
bjorntukkertje 11:aac90fca0290 233 // 110 graden
bjorntukkertje 11:aac90fca0290 234 MotorL1 = 1 ;
bjorntukkertje 11:aac90fca0290 235 MotorL2 = 0 ;
bjorntukkertje 11:aac90fca0290 236 MotorR3 = 0.45 ;
bjorntukkertje 11:aac90fca0290 237 MotorR4 = 0 ;
bjorntukkertje 11:aac90fca0290 238 wait_ms (1000);
bjorntukkertje 11:aac90fca0290 239 }
bjorntukkertje 11:aac90fca0290 240 if (afstand > 0.25 && pos >= 322 && pos < 349) {
bjorntukkertje 11:aac90fca0290 241 // 120 graden
bjorntukkertje 11:aac90fca0290 242 MotorL1 = 1 ;
bjorntukkertje 11:aac90fca0290 243 MotorL2 = 0 ;
bjorntukkertje 11:aac90fca0290 244 MotorR3 = 0.4 ;
bjorntukkertje 11:aac90fca0290 245 MotorR4 = 0 ;
bjorntukkertje 11:aac90fca0290 246 wait_ms (1000);
bjorntukkertje 11:aac90fca0290 247 }
bjorntukkertje 11:aac90fca0290 248 if (afstand > 0.25 && pos >= 349 && pos < 377) {
bjorntukkertje 11:aac90fca0290 249 // 130 graden
bjorntukkertje 11:aac90fca0290 250 MotorL1 = 1 ;
bjorntukkertje 11:aac90fca0290 251 MotorL2 = 0 ;
bjorntukkertje 11:aac90fca0290 252 MotorR3 = 0.35 ;
bjorntukkertje 11:aac90fca0290 253 MotorR4 = 0 ;
bjorntukkertje 11:aac90fca0290 254 wait_ms (1000);
bjorntukkertje 11:aac90fca0290 255 }
bjorntukkertje 11:aac90fca0290 256 if (afstand > 0.25 && pos >= 377 && pos < 405) {
bjorntukkertje 11:aac90fca0290 257 // 140 graden
bjorntukkertje 11:aac90fca0290 258 MotorL1 = 1 ;
bjorntukkertje 11:aac90fca0290 259 MotorL2 = 0 ;
bjorntukkertje 11:aac90fca0290 260 MotorR3 = 0.3 ;
bjorntukkertje 11:aac90fca0290 261 MotorR4 = 0 ;
bjorntukkertje 11:aac90fca0290 262 wait_ms (1000);
bjorntukkertje 11:aac90fca0290 263 }
bjorntukkertje 11:aac90fca0290 264 if (afstand > 0.25 && pos >= 405 && pos < 433) {
bjorntukkertje 11:aac90fca0290 265 // 150 graden
bjorntukkertje 11:aac90fca0290 266 MotorL1 = 1 ;
bjorntukkertje 11:aac90fca0290 267 MotorL2 = 0 ;
bjorntukkertje 11:aac90fca0290 268 MotorR3 = 0.25 ;
bjorntukkertje 11:aac90fca0290 269 MotorR4 = 0 ;
bjorntukkertje 11:aac90fca0290 270 wait_ms (1000);
bjorntukkertje 11:aac90fca0290 271 }
bjorntukkertje 11:aac90fca0290 272 if (afstand > 0.25 && pos >= 433 && pos < 460) {
bjorntukkertje 11:aac90fca0290 273 // 160 graden
bjorntukkertje 11:aac90fca0290 274 MotorL1 = 1 ;
bjorntukkertje 11:aac90fca0290 275 MotorL2 = 0 ;
bjorntukkertje 11:aac90fca0290 276 MotorR3 = 0.2 ;
bjorntukkertje 11:aac90fca0290 277 MotorR4 = 0 ;
bjorntukkertje 11:aac90fca0290 278 wait_ms (1000);
bjorntukkertje 11:aac90fca0290 279 }
bjorntukkertje 11:aac90fca0290 280 if (afstand > 0.25 && pos >= 460 && pos < 488) {
bjorntukkertje 11:aac90fca0290 281 // 170 graden
bjorntukkertje 11:aac90fca0290 282 MotorL1 = 1 ;
bjorntukkertje 11:aac90fca0290 283 MotorL2 = 0 ;
bjorntukkertje 11:aac90fca0290 284 MotorR3 = 0.15 ;
bjorntukkertje 11:aac90fca0290 285 MotorR4 = 0 ;
bjorntukkertje 11:aac90fca0290 286 wait_ms (1000);
bjorntukkertje 11:aac90fca0290 287 }
bjorntukkertje 11:aac90fca0290 288 if (afstand > 0.25 && pos >= 488 && pos < 516) {
bjorntukkertje 11:aac90fca0290 289 // 180 graden
bjorntukkertje 11:aac90fca0290 290 MotorL1 = 1 ;
bjorntukkertje 11:aac90fca0290 291 MotorL2 = 0 ;
bjorntukkertje 11:aac90fca0290 292 MotorR3 = 0.1 ;
bjorntukkertje 11:aac90fca0290 293 MotorR4 = 0 ;
bjorntukkertje 11:aac90fca0290 294 wait_ms (1000);
bjorntukkertje 11:aac90fca0290 295 }
bjorntukkertje 11:aac90fca0290 296 if (afstand > 0.25 && pos >= 516 && pos < 543) {
bjorntukkertje 11:aac90fca0290 297 // 190 graden
bjorntukkertje 11:aac90fca0290 298 MotorL1 = 1 ;
bjorntukkertje 11:aac90fca0290 299 MotorL2 = 0 ;
bjorntukkertje 11:aac90fca0290 300 MotorR3 = 0.15 ;
bjorntukkertje 11:aac90fca0290 301 MotorR4 = 0 ;
bjorntukkertje 11:aac90fca0290 302 wait_ms (1000);
bjorntukkertje 11:aac90fca0290 303 }
bjorntukkertje 11:aac90fca0290 304 if (afstand > 0.25 && pos >= 543 && pos < 571) {
bjorntukkertje 11:aac90fca0290 305 // 200 graden
bjorntukkertje 11:aac90fca0290 306 MotorL1 = 1 ;
bjorntukkertje 11:aac90fca0290 307 MotorL2 = 0 ;
bjorntukkertje 11:aac90fca0290 308 MotorR3 = 0.2 ;
bjorntukkertje 11:aac90fca0290 309 MotorR4 = 0 ;
bjorntukkertje 11:aac90fca0290 310 wait_ms (1000);
bjorntukkertje 11:aac90fca0290 311 }
bjorntukkertje 11:aac90fca0290 312 if (afstand > 0.25 && pos >= 571 && pos < 599) {
bjorntukkertje 11:aac90fca0290 313 // 210 graden
bjorntukkertje 11:aac90fca0290 314 MotorL1 = 1 ;
bjorntukkertje 11:aac90fca0290 315 MotorL2 = 0 ;
bjorntukkertje 11:aac90fca0290 316 MotorR3 = 0.25 ;
bjorntukkertje 11:aac90fca0290 317 MotorR4 = 0 ;
bjorntukkertje 11:aac90fca0290 318 wait_ms (1000);
bjorntukkertje 11:aac90fca0290 319 }
bjorntukkertje 11:aac90fca0290 320 if (afstand > 0.25 && pos >= 599 && pos < 626) {
bjorntukkertje 11:aac90fca0290 321 // 220 graden
bjorntukkertje 11:aac90fca0290 322 MotorL1 = 1 ;
bjorntukkertje 11:aac90fca0290 323 MotorL2 = 0 ;
bjorntukkertje 11:aac90fca0290 324 MotorR3 = 0.30 ;
bjorntukkertje 11:aac90fca0290 325 MotorR4 = 0 ;
bjorntukkertje 11:aac90fca0290 326 wait_ms (1000);
bjorntukkertje 11:aac90fca0290 327 }
bjorntukkertje 11:aac90fca0290 328 if (afstand > 0.25 && pos >= 626 && pos < 654) {
bjorntukkertje 11:aac90fca0290 329 // 230 graden
bjorntukkertje 11:aac90fca0290 330 MotorL1 = 1 ;
bjorntukkertje 11:aac90fca0290 331 MotorL2 = 0 ;
bjorntukkertje 11:aac90fca0290 332 MotorR3 = 0.35 ;
bjorntukkertje 11:aac90fca0290 333 MotorR4 = 0 ;
bjorntukkertje 11:aac90fca0290 334 wait_ms (1000);
bjorntukkertje 11:aac90fca0290 335 }
bjorntukkertje 11:aac90fca0290 336 if (afstand > 0.25 && pos >= 654 && pos < 682) {
bjorntukkertje 11:aac90fca0290 337 // 240 graden
bjorntukkertje 11:aac90fca0290 338 MotorL1 = 1 ;
bjorntukkertje 11:aac90fca0290 339 MotorL2 = 0 ;
bjorntukkertje 11:aac90fca0290 340 MotorR3 = 0.40 ;
bjorntukkertje 11:aac90fca0290 341 MotorR4 = 0 ;
bjorntukkertje 11:aac90fca0290 342 wait_ms (1000);
bjorntukkertje 11:aac90fca0290 343 }
bjorntukkertje 11:aac90fca0290 344 if (afstand > 0.25 && pos >= 682 && pos < 710) {
bjorntukkertje 11:aac90fca0290 345 // 250 graden
bjorntukkertje 11:aac90fca0290 346 MotorL1 = 1 ;
bjorntukkertje 11:aac90fca0290 347 MotorL2 = 0 ;
bjorntukkertje 11:aac90fca0290 348 MotorR3 = 0.45 ;
bjorntukkertje 11:aac90fca0290 349 MotorR4 = 0 ;
bjorntukkertje 11:aac90fca0290 350 wait_ms (1000);
bjorntukkertje 11:aac90fca0290 351 }
bjorntukkertje 11:aac90fca0290 352 if (afstand > 0.25 && pos >= 710 && pos < 737) {
bjorntukkertje 11:aac90fca0290 353 // 260 graden
bjorntukkertje 11:aac90fca0290 354 MotorL1 = 1 ;
bjorntukkertje 11:aac90fca0290 355 MotorL2 = 0 ;
bjorntukkertje 11:aac90fca0290 356 MotorR3 = 0.50 ;
bjorntukkertje 11:aac90fca0290 357 MotorR4 = 0 ;
bjorntukkertje 11:aac90fca0290 358 wait_ms (1000);
bjorntukkertje 11:aac90fca0290 359 }
bjorntukkertje 11:aac90fca0290 360 if (afstand > 0.25 && pos >= 737 && pos < 765) {
bjorntukkertje 11:aac90fca0290 361 // 270 graden
bjorntukkertje 11:aac90fca0290 362 MotorL1 = 1 ;
bjorntukkertje 11:aac90fca0290 363 MotorL2 = 0 ;
bjorntukkertje 11:aac90fca0290 364 MotorR3 = 0.55 ;
bjorntukkertje 11:aac90fca0290 365 MotorR4 = 0 ;
bjorntukkertje 11:aac90fca0290 366 wait_ms (1000);
bjorntukkertje 11:aac90fca0290 367 }
bjorntukkertje 11:aac90fca0290 368 if (afstand > 0.25 && pos >= 765 && pos < 793) {
bjorntukkertje 11:aac90fca0290 369 // 280 graden
bjorntukkertje 11:aac90fca0290 370 MotorL1 = 1 ;
bjorntukkertje 11:aac90fca0290 371 MotorL2 = 0 ;
bjorntukkertje 11:aac90fca0290 372 MotorR3 = 0.60 ;
bjorntukkertje 11:aac90fca0290 373 MotorR4 = 0 ;
bjorntukkertje 11:aac90fca0290 374 wait_ms (1000);
bjorntukkertje 11:aac90fca0290 375 }
bjorntukkertje 11:aac90fca0290 376 if (afstand > 0.25 && pos >= 793 && pos < 820) {
bjorntukkertje 11:aac90fca0290 377 // 290 graden
bjorntukkertje 11:aac90fca0290 378 MotorL1 = 1 ;
bjorntukkertje 11:aac90fca0290 379 MotorL2 = 0 ;
bjorntukkertje 11:aac90fca0290 380 MotorR3 = 0.65 ;
bjorntukkertje 11:aac90fca0290 381 MotorR4 = 0 ;
bjorntukkertje 11:aac90fca0290 382 wait_ms (1000);
bjorntukkertje 11:aac90fca0290 383 }
bjorntukkertje 11:aac90fca0290 384 if (afstand > 0.25 && pos >= 820 && pos < 848) {
bjorntukkertje 11:aac90fca0290 385 // 300 graden
bjorntukkertje 11:aac90fca0290 386 MotorL1 = 1 ;
bjorntukkertje 11:aac90fca0290 387 MotorL2 = 0 ;
bjorntukkertje 11:aac90fca0290 388 MotorR3 = 0.70 ;
bjorntukkertje 11:aac90fca0290 389 MotorR4 = 0 ;
bjorntukkertje 11:aac90fca0290 390 wait_ms (1000);
bjorntukkertje 11:aac90fca0290 391 }
bjorntukkertje 11:aac90fca0290 392 if (afstand > 0.25 && pos >= 848 && pos < 876) {
bjorntukkertje 11:aac90fca0290 393 // 310 graden
bjorntukkertje 11:aac90fca0290 394 MotorL1 = 1 ;
bjorntukkertje 11:aac90fca0290 395 MotorL2 = 0 ;
bjorntukkertje 11:aac90fca0290 396 MotorR3 = 0.75 ;
bjorntukkertje 11:aac90fca0290 397 MotorR4 = 0 ;
bjorntukkertje 11:aac90fca0290 398 wait_ms (1000);
bjorntukkertje 11:aac90fca0290 399 }
bjorntukkertje 11:aac90fca0290 400 if (afstand > 0.25 && pos >= 876 && pos < 903) {
bjorntukkertje 11:aac90fca0290 401 // 320 graden
bjorntukkertje 11:aac90fca0290 402 MotorL1 = 1 ;
bjorntukkertje 11:aac90fca0290 403 MotorL2 = 0 ;
bjorntukkertje 11:aac90fca0290 404 MotorR3 = 0.80 ;
bjorntukkertje 11:aac90fca0290 405 MotorR4 = 0 ;
bjorntukkertje 11:aac90fca0290 406 wait_ms (1000);
bjorntukkertje 11:aac90fca0290 407 }
bjorntukkertje 11:aac90fca0290 408 if (afstand > 0.25 && pos >= 903 && pos < 931) {
bjorntukkertje 11:aac90fca0290 409 // 330 graden
bjorntukkertje 11:aac90fca0290 410 MotorL1 = 1 ;
bjorntukkertje 11:aac90fca0290 411 MotorL2 = 0 ;
bjorntukkertje 11:aac90fca0290 412 MotorR3 = 0.85 ;
bjorntukkertje 11:aac90fca0290 413 MotorR4 = 0 ;
bjorntukkertje 11:aac90fca0290 414 wait_ms (1000);
bjorntukkertje 11:aac90fca0290 415 }
bjorntukkertje 11:aac90fca0290 416 if (afstand > 0.25 && pos >= 931 && pos < 956) {
bjorntukkertje 11:aac90fca0290 417 // 340 graden
bjorntukkertje 11:aac90fca0290 418 MotorL1 = 1 ;
bjorntukkertje 11:aac90fca0290 419 MotorL2 = 0 ;
bjorntukkertje 11:aac90fca0290 420 MotorR3 = 0.90 ;
bjorntukkertje 11:aac90fca0290 421 MotorR4 = 0 ;
bjorntukkertje 11:aac90fca0290 422 wait_ms (1000);
bjorntukkertje 11:aac90fca0290 423 }
bjorntukkertje 11:aac90fca0290 424 if (afstand > 0.25 && pos >= 956 && pos < 987) {
bjorntukkertje 11:aac90fca0290 425 // 350 graden
bjorntukkertje 11:aac90fca0290 426 MotorL1 = 1 ;
bjorntukkertje 11:aac90fca0290 427 MotorL2 = 0 ;
bjorntukkertje 11:aac90fca0290 428 MotorR3 = 0.95 ;
bjorntukkertje 11:aac90fca0290 429 MotorR4 = 0 ;
bjorntukkertje 11:aac90fca0290 430 wait_ms (1000);
bjorntukkertje 11:aac90fca0290 431 }
bjorntukkertje 11:aac90fca0290 432 }
bjorntukkertje 0:9756a10feb5f 433 }