project enzo / Mbed 2 deprecated robot Featured

dit is em

Dependencies:   mbed

main.cpp@16:a9382753e8d6, 2017-05-16 (annotated)

Committer:
joosthartkamp
Date:
Tue May 16 14:07:43 2017 +0000
Revision:
16:a9382753e8d6
Parent:
14:878d20f36e99
Child:
17:9bbd95969b6a
Child:
18:3a613ea8e28c
nucleo naait ons

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bjorntukkertje 0:9756a10feb5f 1 #include "mbed.h"
bjorntukkertje 0:9756a10feb5f 2
joosthartkamp 16:a9382753e8d6 3 PwmOut motorleftspeed(D1);
joosthartkamp 16:a9382753e8d6 4 PwmOut motorrightspeed(D7);
joosthartkamp 16:a9382753e8d6 5 DigitalOut MotorL1pin (D13);
joosthartkamp 16:a9382753e8d6 6 DigitalOut MotorL2pin (D0);
joosthartkamp 16:a9382753e8d6 7 DigitalOut MotorR1pin (D12);
joosthartkamp 16:a9382753e8d6 8 DigitalOut MotorR2pin (D11);
bjorntukkertje 6:6659a8ada914 9 AnalogIn Sensor1 (A1);
bjorntukkertje 6:6659a8ada914 10 AnalogIn Sensor2 (A2);
bjorntukkertje 6:6659a8ada914 11 AnalogIn Sensor3 (A3);
bjorntukkertje 6:6659a8ada914 12 AnalogIn Sensor4 (A4);
bjorntukkertje 11:aac90fca0290 13 AnalogIn zoeksensor (A5);
joosthartkamp 16:a9382753e8d6 14 AnalogIn currentleftpin (A6);
joosthartkamp 16:a9382753e8d6 15 AnalogIn currentrightpin (A7);
bjorntukkertje 10:18116d979d61 16 DigitalOut steppera (D2);
bjorntukkertje 10:18116d979d61 17 DigitalOut stepperb (D3);
bjorntukkertje 10:18116d979d61 18 DigitalOut stepperc (D4);
bjorntukkertje 10:18116d979d61 19 DigitalOut stepperd (D5);
bjorntukkertje 4:038346651cc0 20
bjorntukkertje 0:9756a10feb5f 21
bjorntukkertje 10:18116d979d61 22 Ticker lidar;
bjorntukkertje 10:18116d979d61 23 Ticker serial;
bjorntukkertje 12:2f3afa8d0ddc 24 Timer T;
bjorntukkertje 10:18116d979d61 25 Serial pc(USBTX, USBRX);
bjorntukkertje 0:9756a10feb5f 26
bjorntukkertje 11:aac90fca0290 27 int afstand = zoeksensor;
bjorntukkertje 11:aac90fca0290 28
bjorntukkertje 10:18116d979d61 29 int stapmode = 0;
bjorntukkertje 10:18116d979d61 30 int position = 0;
bjorntukkertje 10:18116d979d61 31 int pos;
bjorntukkertje 10:18116d979d61 32
joosthartkamp 16:a9382753e8d6 33 int mapcurrent(float input,float inputmin, float inputmax,int outputmin, int outputmax)
joosthartkamp 16:a9382753e8d6 34 {
joosthartkamp 16:a9382753e8d6 35 return (input - inputmin) * (outputmax - outputmin) / (inputmax - inputmin) + outputmin;
joosthartkamp 16:a9382753e8d6 36 }
bjorntukkertje 10:18116d979d61 37 // poar neemn
bjorntukkertje 10:18116d979d61 38 // twee poar neemn
bjorntukkertje 10:18116d979d61 39 // twee tettn in n envelop
bjorntukkertje 10:18116d979d61 40
joosthartkamp 16:a9382753e8d6 41 bool afstandzoeker ()
bjorntukkertje 13:02e56051501a 42 {
bjorntukkertje 13:02e56051501a 43 bool a ;
bjorntukkertje 13:02e56051501a 44 if (zoeksensor > 0.25) {
bjorntukkertje 13:02e56051501a 45 a = 1;
joosthartkamp 16:a9382753e8d6 46 } else {
bjorntukkertje 13:02e56051501a 47 a=0 ;
bjorntukkertje 13:02e56051501a 48 }
bjorntukkertje 13:02e56051501a 49 return a;
bjorntukkertje 13:02e56051501a 50 }
bjorntukkertje 13:02e56051501a 51
bjorntukkertje 13:02e56051501a 52 int lijnsensor ()
bjorntukkertje 13:02e56051501a 53
bjorntukkertje 13:02e56051501a 54 {
bjorntukkertje 13:02e56051501a 55 bool a = 0;
bjorntukkertje 13:02e56051501a 56 bool b = 0;
bjorntukkertje 13:02e56051501a 57 bool c = 0;
bjorntukkertje 13:02e56051501a 58 bool d = 0;
bjorntukkertje 13:02e56051501a 59 bool e = 0;
bjorntukkertje 13:02e56051501a 60 bool f = 0;
bjorntukkertje 13:02e56051501a 61 int output;
bjorntukkertje 13:02e56051501a 62 if (Sensor1 > 0.01) {
bjorntukkertje 13:02e56051501a 63 a = 1;
bjorntukkertje 13:02e56051501a 64 } else {
bjorntukkertje 13:02e56051501a 65 a = 0;
bjorntukkertje 13:02e56051501a 66 }
bjorntukkertje 13:02e56051501a 67 if (Sensor2 > 0.01) {
bjorntukkertje 13:02e56051501a 68 b = 1<<1;
bjorntukkertje 13:02e56051501a 69 } else {
bjorntukkertje 13:02e56051501a 70 b = 0<<1;
bjorntukkertje 13:02e56051501a 71 }
bjorntukkertje 13:02e56051501a 72 if (Sensor3 > 0.01) {
bjorntukkertje 13:02e56051501a 73 c = 1<<2;
bjorntukkertje 13:02e56051501a 74 } else {
bjorntukkertje 13:02e56051501a 75 c = 0<<2;
bjorntukkertje 13:02e56051501a 76 }
bjorntukkertje 13:02e56051501a 77 if (Sensor4 > 0.01) {
bjorntukkertje 13:02e56051501a 78 d = 1<<3;
bjorntukkertje 13:02e56051501a 79 } else {
bjorntukkertje 13:02e56051501a 80 d = 0<<3;
bjorntukkertje 13:02e56051501a 81 }
joosthartkamp 16:a9382753e8d6 82
bjorntukkertje 14:878d20f36e99 83 output = a || b || c || d;
joosthartkamp 16:a9382753e8d6 84 return output;
bjorntukkertje 13:02e56051501a 85 }
bjorntukkertje 13:02e56051501a 86
joosthartkamp 16:a9382753e8d6 87 void(int speedleft, int speedright, int direction){
joosthartkamp 16:a9382753e8d6 88 int direction = 0;
joosthartkamp 16:a9382753e8d6 89 int currenta = mapcurrent(currentleftpin,0,1,0,6600);
joosthartkamp 16:a9382753e8d6 90 int currentb = mapcurrent(currentrightpin,0,1,0,6600);
joosthartkamp 16:a9382753e8d6 91 int output[] = (currenta,currentb);
joosthartkamp 16:a9382753e8d6 92
joosthartkamp 16:a9382753e8d6 93 bool MotorL1 = 0;
joosthartkamp 16:a9382753e8d6 94 bool MotorL2 = 0;
joosthartkamp 16:a9382753e8d6 95 bool MotorR1 = 0;
joosthartkamp 16:a9382753e8d6 96 bool MotorR2 = 0;
joosthartkamp 16:a9382753e8d6 97 switch (direction) {
joosthartkamp 16:a9382753e8d6 98 case 0:
joosthartkamp 16:a9382753e8d6 99 MotorL1 = 0;
joosthartkamp 16:a9382753e8d6 100 MotorL2 = 0;
joosthartkamp 16:a9382753e8d6 101 MotorR1 = 0;
joosthartkamp 16:a9382753e8d6 102 MotorR2 = 0;
joosthartkamp 16:a9382753e8d6 103 break;
joosthartkamp 16:a9382753e8d6 104 case 1:
joosthartkamp 16:a9382753e8d6 105 MotorL1 = 1;
joosthartkamp 16:a9382753e8d6 106 MotorL2 = 0;
joosthartkamp 16:a9382753e8d6 107 MotorR1 = 1;
joosthartkamp 16:a9382753e8d6 108 MotorR2 = 0;
joosthartkamp 16:a9382753e8d6 109 break;
joosthartkamp 16:a9382753e8d6 110 case 2:
joosthartkamp 16:a9382753e8d6 111 MotorL1 = 0;
joosthartkamp 16:a9382753e8d6 112 MotorL2 = 1;
joosthartkamp 16:a9382753e8d6 113 MotorR1 = 0;
joosthartkamp 16:a9382753e8d6 114 MotorR2 = 1;
joosthartkamp 16:a9382753e8d6 115 break;
joosthartkamp 16:a9382753e8d6 116 case 3:
joosthartkamp 16:a9382753e8d6 117 MotorL1 = 0;
joosthartkamp 16:a9382753e8d6 118 MotorL2 = 1;
joosthartkamp 16:a9382753e8d6 119 MotorR1 = 1;
joosthartkamp 16:a9382753e8d6 120 MotorR2 = 0;
joosthartkamp 16:a9382753e8d6 121 break;
joosthartkamp 16:a9382753e8d6 122 case 4:
joosthartkamp 16:a9382753e8d6 123 MotorL1 = 1;
joosthartkamp 16:a9382753e8d6 124 MotorL2 = 0;
joosthartkamp 16:a9382753e8d6 125 MotorR1 = 0;
joosthartkamp 16:a9382753e8d6 126 MotorR2 = 1;
joosthartkamp 16:a9382753e8d6 127 break;
joosthartkamp 16:a9382753e8d6 128 case 5:
joosthartkamp 16:a9382753e8d6 129 MotorL1 = 0;
joosthartkamp 16:a9382753e8d6 130 MotorL2 = 0;
joosthartkamp 16:a9382753e8d6 131 MotorR1 = 1;
joosthartkamp 16:a9382753e8d6 132 MotorR2 = 0;
joosthartkamp 16:a9382753e8d6 133 break;
joosthartkamp 16:a9382753e8d6 134 case 6:
joosthartkamp 16:a9382753e8d6 135 MotorL1 = 1;
joosthartkamp 16:a9382753e8d6 136 MotorL2 = 0;
joosthartkamp 16:a9382753e8d6 137 MotorR1 = 0;
joosthartkamp 16:a9382753e8d6 138 MotorR2 = 0;
joosthartkamp 16:a9382753e8d6 139 break;
joosthartkamp 16:a9382753e8d6 140 case 7:
joosthartkamp 16:a9382753e8d6 141 MotorL1 = 0;
joosthartkamp 16:a9382753e8d6 142 MotorL2 = 0;
joosthartkamp 16:a9382753e8d6 143 MotorR1 = 0;
joosthartkamp 16:a9382753e8d6 144 MotorR2 = 1;
joosthartkamp 16:a9382753e8d6 145 break;
joosthartkamp 16:a9382753e8d6 146 case 8:
joosthartkamp 16:a9382753e8d6 147 MotorL1 = 0;
joosthartkamp 16:a9382753e8d6 148 MotorL2 = 1;
joosthartkamp 16:a9382753e8d6 149 MotorR1 = 0;
joosthartkamp 16:a9382753e8d6 150 MotorR2 = 0;
joosthartkamp 16:a9382753e8d6 151 break;
joosthartkamp 16:a9382753e8d6 152
joosthartkamp 16:a9382753e8d6 153 }
joosthartkamp 16:a9382753e8d6 154
joosthartkamp 16:a9382753e8d6 155 }
joosthartkamp 16:a9382753e8d6 156 /*
bjorntukkertje 14:878d20f36e99 157 int vooruit() {
bjorntukkertje 14:878d20f36e99 158 MotorL1 = 1;
bjorntukkertje 14:878d20f36e99 159 MotorL2 = 0;
bjorntukkertje 14:878d20f36e99 160 MotorR1 = 1;
bjorntukkertje 14:878d20f36e99 161 MotorR2 = 0;
joosthartkamp 16:a9382753e8d6 162
bjorntukkertje 14:878d20f36e99 163 }
bjorntukkertje 14:878d20f36e99 164 int achteruit () {
bjorntukkertje 14:878d20f36e99 165 MotorL1 = 0;
bjorntukkertje 14:878d20f36e99 166 MotorL2 = 1;
bjorntukkertje 14:878d20f36e99 167 MotorR1 = 0;
bjorntukkertje 14:878d20f36e99 168 MotorR2 = 1;
bjorntukkertje 14:878d20f36e99 169 }
bjorntukkertje 14:878d20f36e99 170 int Hardebochtlinksom (){
bjorntukkertje 14:878d20f36e99 171 MotorL1 = 1;
bjorntukkertje 14:878d20f36e99 172 MotorL2 = 0;
bjorntukkertje 14:878d20f36e99 173 MotorR1 = 0;
bjorntukkertje 14:878d20f36e99 174 MotorR2 = 1;
bjorntukkertje 14:878d20f36e99 175 }
bjorntukkertje 14:878d20f36e99 176 int Hardebochtrechtsom() {
bjorntukkertje 14:878d20f36e99 177 MotorL1 = 0;
bjorntukkertje 14:878d20f36e99 178 MotorL2 = 1;
bjorntukkertje 14:878d20f36e99 179 MotorR1 = 1;
bjorntukkertje 14:878d20f36e99 180 MotorR2 = 0;
bjorntukkertje 14:878d20f36e99 181 }
joosthartkamp 16:a9382753e8d6 182 */
bjorntukkertje 13:02e56051501a 183
joosthartkamp 16:a9382753e8d6 184 int stepper()
bjorntukkertje 10:18116d979d61 185 {
bjorntukkertje 10:18116d979d61 186 switch (stapmode) {
bjorntukkertje 10:18116d979d61 187 case 0:
bjorntukkertje 10:18116d979d61 188 steppera = 1;
bjorntukkertje 10:18116d979d61 189 stepperb = 0;
bjorntukkertje 10:18116d979d61 190 stepperc = 1;
bjorntukkertje 10:18116d979d61 191 stepperd = 0;
bjorntukkertje 10:18116d979d61 192 pos ++;
bjorntukkertje 10:18116d979d61 193 break;
bjorntukkertje 10:18116d979d61 194 case 1:
bjorntukkertje 10:18116d979d61 195 steppera = 1;
bjorntukkertje 10:18116d979d61 196 stepperb = 0;
bjorntukkertje 10:18116d979d61 197 stepperc = 0;
bjorntukkertje 10:18116d979d61 198 stepperd = 1;
bjorntukkertje 10:18116d979d61 199 pos ++;
bjorntukkertje 10:18116d979d61 200 break;
bjorntukkertje 10:18116d979d61 201 case 2:
bjorntukkertje 10:18116d979d61 202 steppera = 0;
bjorntukkertje 10:18116d979d61 203 stepperb = 1;
bjorntukkertje 10:18116d979d61 204 stepperc = 0;
bjorntukkertje 10:18116d979d61 205 stepperd = 1;
bjorntukkertje 10:18116d979d61 206 pos ++;
bjorntukkertje 10:18116d979d61 207 break;
bjorntukkertje 10:18116d979d61 208 case 3:
bjorntukkertje 10:18116d979d61 209 steppera = 0;
bjorntukkertje 10:18116d979d61 210 stepperb = 1;
bjorntukkertje 10:18116d979d61 211 stepperc = 1;
bjorntukkertje 10:18116d979d61 212 stepperd = 0;
bjorntukkertje 10:18116d979d61 213 pos ++;
bjorntukkertje 10:18116d979d61 214 break;
bjorntukkertje 10:18116d979d61 215 ;
bjorntukkertje 10:18116d979d61 216 }
bjorntukkertje 10:18116d979d61 217 if (pos > 1000) { //na volledige rotatie ga naar nul
bjorntukkertje 13:02e56051501a 218 pos = 0;
bjorntukkertje 13:02e56051501a 219 }
joosthartkamp 16:a9382753e8d6 220 stapmode ++;
joosthartkamp 16:a9382753e8d6 221 if (stapmode > 3) {
joosthartkamp 16:a9382753e8d6 222 stapmode = 0;
joosthartkamp 16:a9382753e8d6 223 }
bjorntukkertje 10:18116d979d61 224 return pos;
bjorntukkertje 13:02e56051501a 225
bjorntukkertje 10:18116d979d61 226 }
bjorntukkertje 13:02e56051501a 227
joosthartkamp 16:a9382753e8d6 228 /*
bjorntukkertje 10:18116d979d61 229 void run_serial() //deze functie word periodiek aangeroepen om de stappenmotor te draaien en de afstand te meten
bjorntukkertje 10:18116d979d61 230 {
bjorntukkertje 13:02e56051501a 231
bjorntukkertje 10:18116d979d61 232 pc.printf("%d\n",position);
bjorntukkertje 10:18116d979d61 233 }
bjorntukkertje 13:02e56051501a 234
bjorntukkertje 10:18116d979d61 235 void run_lidar() //deze functie word periodiek aangeroepen om de stappenmotor te draaien en de afstand te meten
bjorntukkertje 10:18116d979d61 236 {
bjorntukkertje 10:18116d979d61 237 stapmode ++;
bjorntukkertje 10:18116d979d61 238 if (stapmode > 3) {
bjorntukkertje 10:18116d979d61 239 stapmode = 0;
bjorntukkertje 10:18116d979d61 240 }
bjorntukkertje 10:18116d979d61 241 position = stepper(stapmode);
bjorntukkertje 10:18116d979d61 242 }
bjorntukkertje 13:02e56051501a 243
bjorntukkertje 8:ec7b7c9ad07e 244
bjorntukkertje 13:02e56051501a 245 int main()
bjorntukkertje 13:02e56051501a 246 {
bjorntukkertje 10:18116d979d61 247 lidar.attach(&run_lidar, 0.0001);
bjorntukkertje 10:18116d979d61 248 serial.attach(&run_serial, 0.2);
bjorntukkertje 13:02e56051501a 249
bjorntukkertje 0:9756a10feb5f 250 while(1) {
bjorntukkertje 8:ec7b7c9ad07e 251 if ( Sensor1 >= 1) {
bjorntukkertje 13:02e56051501a 252 MotorL1 = 0 ;
bjorntukkertje 13:02e56051501a 253 MotorL2 = 1 ;
bjorntukkertje 13:02e56051501a 254 MotorR3 = 0 ;
bjorntukkertje 13:02e56051501a 255 MotorR4 = 1 ;
bjorntukkertje 13:02e56051501a 256 wait_ms (500);
bjorntukkertje 13:02e56051501a 257 MotorL1 = 1 ;
bjorntukkertje 13:02e56051501a 258 MotorL2 = 0 ;
bjorntukkertje 13:02e56051501a 259 MotorR3 = 0 ;
bjorntukkertje 13:02e56051501a 260 MotorR4 = 1 ;
bjorntukkertje 13:02e56051501a 261 wait_ms (1000);
bjorntukkertje 13:02e56051501a 262 }
bjorntukkertje 8:ec7b7c9ad07e 263 if (Sensor2 >= 1) {
bjorntukkertje 13:02e56051501a 264 MotorL1 = 0 ;
bjorntukkertje 13:02e56051501a 265 MotorL2 = 1 ;
bjorntukkertje 13:02e56051501a 266 MotorR3 = 0 ;
bjorntukkertje 13:02e56051501a 267 MotorR4 = 1 ;
bjorntukkertje 13:02e56051501a 268 wait_ms (500);
bjorntukkertje 13:02e56051501a 269 MotorL1 = 1 ;
bjorntukkertje 13:02e56051501a 270 MotorL2 = 0 ;
bjorntukkertje 13:02e56051501a 271 MotorR3 = 0 ;
bjorntukkertje 13:02e56051501a 272 MotorR4 = 1 ;
bjorntukkertje 13:02e56051501a 273 wait_ms (1000);
bjorntukkertje 13:02e56051501a 274 }
bjorntukkertje 8:ec7b7c9ad07e 275 if (Sensor3 >= 1) {
bjorntukkertje 13:02e56051501a 276 MotorL1 = 0 ;
bjorntukkertje 13:02e56051501a 277 MotorL2 = 1 ;
bjorntukkertje 13:02e56051501a 278 MotorR3 = 0 ;
bjorntukkertje 13:02e56051501a 279 MotorR4 = 1 ;
bjorntukkertje 13:02e56051501a 280 wait_ms (500);
bjorntukkertje 13:02e56051501a 281 MotorL1 = 1 ;
bjorntukkertje 13:02e56051501a 282 MotorL2 = 0 ;
bjorntukkertje 13:02e56051501a 283 MotorR3 = 0 ;
bjorntukkertje 13:02e56051501a 284 MotorR4 = 1 ;
bjorntukkertje 13:02e56051501a 285 wait_ms (1000);
bjorntukkertje 13:02e56051501a 286 }
bjorntukkertje 8:ec7b7c9ad07e 287 if (Sensor4 >= 1) {
bjorntukkertje 13:02e56051501a 288 MotorL1 = 0 ;
bjorntukkertje 13:02e56051501a 289 MotorL2 = 1 ;
bjorntukkertje 13:02e56051501a 290 MotorR3 = 0 ;
bjorntukkertje 13:02e56051501a 291 MotorR4 = 1 ;
bjorntukkertje 13:02e56051501a 292 wait_ms (500);
bjorntukkertje 13:02e56051501a 293 MotorL1 = 1 ;
bjorntukkertje 13:02e56051501a 294 MotorL2 = 0 ;
bjorntukkertje 13:02e56051501a 295 MotorR3 = 0 ;
bjorntukkertje 13:02e56051501a 296 MotorR4 = 1 ;
bjorntukkertje 13:02e56051501a 297 wait_ms (1000);
bjorntukkertje 13:02e56051501a 298 } else {
bjorntukkertje 11:aac90fca0290 299 MotorL1 = 1 ;
bjorntukkertje 13:02e56051501a 300 MotorL2 = 0 ;
bjorntukkertje 13:02e56051501a 301 MotorR3 = 1 ;
bjorntukkertje 13:02e56051501a 302 MotorR4 = 0 ;
bjorntukkertje 13:02e56051501a 303 }
bjorntukkertje 13:02e56051501a 304
bjorntukkertje 13:02e56051501a 305 // tweede loop
bjorntukkertje 13:02e56051501a 306
bjorntukkertje 13:02e56051501a 307
bjorntukkertje 13:02e56051501a 308 if (afstand > 0.25 && pos > 17 && pos < 45) {
bjorntukkertje 13:02e56051501a 309 // 10 graden
bjorntukkertje 13:02e56051501a 310 MotorL1 = 1 ;
bjorntukkertje 13:02e56051501a 311 MotorL2 = 0 ;
bjorntukkertje 13:02e56051501a 312 MotorR3 = 0.95 ;
bjorntukkertje 13:02e56051501a 313 MotorR4 = 0 ;
bjorntukkertje 13:02e56051501a 314 wait_ms (1000);
bjorntukkertje 13:02e56051501a 315 }
bjorntukkertje 13:02e56051501a 316 if (afstand > 0.25 && pos >= 45&& pos < 72) {
bjorntukkertje 13:02e56051501a 317 // 20 graden
bjorntukkertje 13:02e56051501a 318 MotorL1 = 1 ;
bjorntukkertje 13:02e56051501a 319 MotorL2 = 0 ;
bjorntukkertje 13:02e56051501a 320 MotorR3 = 0.9 ;
bjorntukkertje 13:02e56051501a 321 MotorR4 = 0 ;
bjorntukkertje 13:02e56051501a 322 wait_ms (1000);
bjorntukkertje 13:02e56051501a 323 }
bjorntukkertje 13:02e56051501a 324 if (afstand > 0.25 && pos >= 72 && pos < 100) {
bjorntukkertje 13:02e56051501a 325 // 30 graden
bjorntukkertje 11:aac90fca0290 326 MotorL1 = 1 ;
bjorntukkertje 13:02e56051501a 327 MotorL2 = 0 ;
bjorntukkertje 13:02e56051501a 328 MotorR3 = 0.85 ;
bjorntukkertje 13:02e56051501a 329 MotorR4 = 0 ;
bjorntukkertje 13:02e56051501a 330 wait_ms (1000);
bjorntukkertje 13:02e56051501a 331 }
bjorntukkertje 13:02e56051501a 332 if (afstand > 0.25 && pos >= 100 && pos < 128) {
bjorntukkertje 13:02e56051501a 333 // 40 graden
bjorntukkertje 11:aac90fca0290 334 MotorL1 = 1 ;
bjorntukkertje 13:02e56051501a 335 MotorL2 = 0 ;
bjorntukkertje 13:02e56051501a 336 MotorR3 = 0.8 ;
bjorntukkertje 13:02e56051501a 337 MotorR4 = 0 ;
bjorntukkertje 13:02e56051501a 338 wait_ms (1000);
bjorntukkertje 13:02e56051501a 339 }
bjorntukkertje 13:02e56051501a 340 if (afstand > 0.25 && pos >= 128 && pos < 156) {
bjorntukkertje 13:02e56051501a 341 // 50 graden
bjorntukkertje 11:aac90fca0290 342 MotorL1 = 1 ;
bjorntukkertje 13:02e56051501a 343 MotorL2 = 0 ;
bjorntukkertje 13:02e56051501a 344 MotorR3 = 0.75 ;
bjorntukkertje 13:02e56051501a 345 MotorR4 = 0 ;
bjorntukkertje 13:02e56051501a 346 wait_ms (1000);
bjorntukkertje 13:02e56051501a 347 }
bjorntukkertje 13:02e56051501a 348 if (afstand > 0.25 && pos >= 156 && pos < 183) {
bjorntukkertje 13:02e56051501a 349 // 60 graden
bjorntukkertje 11:aac90fca0290 350 MotorL1 = 1 ;
bjorntukkertje 13:02e56051501a 351 MotorL2 = 0 ;
bjorntukkertje 13:02e56051501a 352 MotorR3 = 0.7 ;
bjorntukkertje 13:02e56051501a 353 MotorR4 = 0 ;
bjorntukkertje 13:02e56051501a 354 wait_ms (1000);
bjorntukkertje 13:02e56051501a 355 }
bjorntukkertje 13:02e56051501a 356 if (afstand > 0.25 && pos >= 183 && pos < 211) {
bjorntukkertje 13:02e56051501a 357 // 70 graden
bjorntukkertje 11:aac90fca0290 358 MotorL1 = 1 ;
bjorntukkertje 13:02e56051501a 359 MotorL2 = 0 ;
bjorntukkertje 13:02e56051501a 360 MotorR3 = 0.65 ;
bjorntukkertje 13:02e56051501a 361 MotorR4 = 0 ;
bjorntukkertje 13:02e56051501a 362 wait_ms (1000);
bjorntukkertje 13:02e56051501a 363 }
bjorntukkertje 13:02e56051501a 364 if (afstand > 0.25 && pos >= 211 && pos < 239) {
bjorntukkertje 13:02e56051501a 365 // 80 graden
bjorntukkertje 11:aac90fca0290 366 MotorL1 = 1 ;
bjorntukkertje 13:02e56051501a 367 MotorL2 = 0 ;
bjorntukkertje 13:02e56051501a 368 MotorR3 = 0.6 ;
bjorntukkertje 13:02e56051501a 369 MotorR4 = 0 ;
bjorntukkertje 13:02e56051501a 370 wait_ms (1000);
bjorntukkertje 13:02e56051501a 371 }
bjorntukkertje 13:02e56051501a 372 if (afstand > 0.25 && pos >= 239 && pos < 266) {
bjorntukkertje 13:02e56051501a 373 // 90 graden
bjorntukkertje 11:aac90fca0290 374 MotorL1 = 1 ;
bjorntukkertje 13:02e56051501a 375 MotorL2 = 0 ;
bjorntukkertje 13:02e56051501a 376 MotorR3 = 0.55 ;
bjorntukkertje 13:02e56051501a 377 MotorR4 = 0 ;
bjorntukkertje 13:02e56051501a 378 wait_ms (1000);
bjorntukkertje 13:02e56051501a 379 }
bjorntukkertje 13:02e56051501a 380 if (afstand > 0.25 && pos >= 266 && pos < 294) {
bjorntukkertje 13:02e56051501a 381 // 100 graden
bjorntukkertje 11:aac90fca0290 382 MotorL1 = 1 ;
bjorntukkertje 13:02e56051501a 383 MotorL2 = 0 ;
bjorntukkertje 13:02e56051501a 384 MotorR3 = 0.5 ;
bjorntukkertje 13:02e56051501a 385 MotorR4 = 0 ;
bjorntukkertje 13:02e56051501a 386 wait_ms (1000);
bjorntukkertje 13:02e56051501a 387 }
bjorntukkertje 13:02e56051501a 388 if (afstand > 0.25 && pos >= 294 && pos < 322) {
bjorntukkertje 13:02e56051501a 389 // 110 graden
bjorntukkertje 11:aac90fca0290 390 MotorL1 = 1 ;
bjorntukkertje 13:02e56051501a 391 MotorL2 = 0 ;
bjorntukkertje 13:02e56051501a 392 MotorR3 = 0.45 ;
bjorntukkertje 13:02e56051501a 393 MotorR4 = 0 ;
bjorntukkertje 13:02e56051501a 394 wait_ms (1000);
bjorntukkertje 13:02e56051501a 395 }
bjorntukkertje 13:02e56051501a 396 if (afstand > 0.25 && pos >= 322 && pos < 349) {
bjorntukkertje 13:02e56051501a 397 // 120 graden
bjorntukkertje 11:aac90fca0290 398 MotorL1 = 1 ;
bjorntukkertje 13:02e56051501a 399 MotorL2 = 0 ;
bjorntukkertje 13:02e56051501a 400 MotorR3 = 0.4 ;
bjorntukkertje 13:02e56051501a 401 MotorR4 = 0 ;
bjorntukkertje 13:02e56051501a 402 wait_ms (1000);
bjorntukkertje 13:02e56051501a 403 }
bjorntukkertje 13:02e56051501a 404 if (afstand > 0.25 && pos >= 349 && pos < 377) {
bjorntukkertje 13:02e56051501a 405 // 130 graden
bjorntukkertje 11:aac90fca0290 406 MotorL1 = 1 ;
bjorntukkertje 13:02e56051501a 407 MotorL2 = 0 ;
bjorntukkertje 13:02e56051501a 408 MotorR3 = 0.35 ;
bjorntukkertje 13:02e56051501a 409 MotorR4 = 0 ;
bjorntukkertje 13:02e56051501a 410 wait_ms (1000);
bjorntukkertje 13:02e56051501a 411 }
bjorntukkertje 13:02e56051501a 412 if (afstand > 0.25 && pos >= 377 && pos < 405) {
bjorntukkertje 13:02e56051501a 413 // 140 graden
bjorntukkertje 11:aac90fca0290 414 MotorL1 = 1 ;
bjorntukkertje 13:02e56051501a 415 MotorL2 = 0 ;
bjorntukkertje 13:02e56051501a 416 MotorR3 = 0.3 ;
bjorntukkertje 13:02e56051501a 417 MotorR4 = 0 ;
bjorntukkertje 13:02e56051501a 418 wait_ms (1000);
bjorntukkertje 13:02e56051501a 419 }
bjorntukkertje 13:02e56051501a 420 if (afstand > 0.25 && pos >= 405 && pos < 433) {
bjorntukkertje 13:02e56051501a 421 // 150 graden
bjorntukkertje 11:aac90fca0290 422 MotorL1 = 1 ;
bjorntukkertje 13:02e56051501a 423 MotorL2 = 0 ;
bjorntukkertje 13:02e56051501a 424 MotorR3 = 0.25 ;
bjorntukkertje 13:02e56051501a 425 MotorR4 = 0 ;
bjorntukkertje 13:02e56051501a 426 wait_ms (1000);
bjorntukkertje 13:02e56051501a 427 }
bjorntukkertje 13:02e56051501a 428 if (afstand > 0.25 && pos >= 433 && pos < 460) {
bjorntukkertje 13:02e56051501a 429 // 160 graden
bjorntukkertje 11:aac90fca0290 430 MotorL1 = 1 ;
bjorntukkertje 13:02e56051501a 431 MotorL2 = 0 ;
bjorntukkertje 13:02e56051501a 432 MotorR3 = 0.2 ;
bjorntukkertje 13:02e56051501a 433 MotorR4 = 0 ;
bjorntukkertje 13:02e56051501a 434 wait_ms (1000);
bjorntukkertje 13:02e56051501a 435 }
bjorntukkertje 13:02e56051501a 436 if (afstand > 0.25 && pos >= 460 && pos < 488) {
bjorntukkertje 13:02e56051501a 437 // 170 graden
bjorntukkertje 11:aac90fca0290 438 MotorL1 = 1 ;
bjorntukkertje 13:02e56051501a 439 MotorL2 = 0 ;
bjorntukkertje 13:02e56051501a 440 MotorR3 = 0.15 ;
bjorntukkertje 13:02e56051501a 441 MotorR4 = 0 ;
bjorntukkertje 13:02e56051501a 442 wait_ms (1000);
bjorntukkertje 13:02e56051501a 443 }
bjorntukkertje 13:02e56051501a 444 if (afstand > 0.25 && pos >= 488 && pos < 516) {
bjorntukkertje 13:02e56051501a 445 // 180 graden
bjorntukkertje 11:aac90fca0290 446 MotorL1 = 1 ;
bjorntukkertje 13:02e56051501a 447 MotorL2 = 0 ;
bjorntukkertje 13:02e56051501a 448 MotorR3 = 0.1 ;
bjorntukkertje 13:02e56051501a 449 MotorR4 = 0 ;
bjorntukkertje 13:02e56051501a 450 wait_ms (1000);
bjorntukkertje 13:02e56051501a 451 }
bjorntukkertje 13:02e56051501a 452 if (afstand > 0.25 && pos >= 516 && pos < 543) {
bjorntukkertje 13:02e56051501a 453 // 190 graden
bjorntukkertje 11:aac90fca0290 454 MotorL1 = 1 ;
bjorntukkertje 13:02e56051501a 455 MotorL2 = 0 ;
bjorntukkertje 13:02e56051501a 456 MotorR3 = 0.15 ;
bjorntukkertje 13:02e56051501a 457 MotorR4 = 0 ;
bjorntukkertje 13:02e56051501a 458 wait_ms (1000);
bjorntukkertje 13:02e56051501a 459 }
bjorntukkertje 13:02e56051501a 460 if (afstand > 0.25 && pos >= 543 && pos < 571) {
bjorntukkertje 13:02e56051501a 461 // 200 graden
bjorntukkertje 11:aac90fca0290 462 MotorL1 = 1 ;
bjorntukkertje 13:02e56051501a 463 MotorL2 = 0 ;
bjorntukkertje 13:02e56051501a 464 MotorR3 = 0.2 ;
bjorntukkertje 13:02e56051501a 465 MotorR4 = 0 ;
bjorntukkertje 13:02e56051501a 466 wait_ms (1000);
bjorntukkertje 13:02e56051501a 467 }
bjorntukkertje 13:02e56051501a 468 if (afstand > 0.25 && pos >= 571 && pos < 599) {
bjorntukkertje 13:02e56051501a 469 // 210 graden
bjorntukkertje 11:aac90fca0290 470 MotorL1 = 1 ;
bjorntukkertje 13:02e56051501a 471 MotorL2 = 0 ;
bjorntukkertje 13:02e56051501a 472 MotorR3 = 0.25 ;
bjorntukkertje 13:02e56051501a 473 MotorR4 = 0 ;
bjorntukkertje 13:02e56051501a 474 wait_ms (1000);
bjorntukkertje 13:02e56051501a 475 }
bjorntukkertje 13:02e56051501a 476 if (afstand > 0.25 && pos >= 599 && pos < 626) {
bjorntukkertje 13:02e56051501a 477 // 220 graden
bjorntukkertje 11:aac90fca0290 478 MotorL1 = 1 ;
bjorntukkertje 13:02e56051501a 479 MotorL2 = 0 ;
bjorntukkertje 13:02e56051501a 480 MotorR3 = 0.30 ;
bjorntukkertje 13:02e56051501a 481 MotorR4 = 0 ;
bjorntukkertje 13:02e56051501a 482 wait_ms (1000);
bjorntukkertje 13:02e56051501a 483 }
bjorntukkertje 13:02e56051501a 484 if (afstand > 0.25 && pos >= 626 && pos < 654) {
bjorntukkertje 13:02e56051501a 485 // 230 graden
bjorntukkertje 11:aac90fca0290 486 MotorL1 = 1 ;
bjorntukkertje 13:02e56051501a 487 MotorL2 = 0 ;
bjorntukkertje 13:02e56051501a 488 MotorR3 = 0.35 ;
bjorntukkertje 13:02e56051501a 489 MotorR4 = 0 ;
bjorntukkertje 13:02e56051501a 490 wait_ms (1000);
bjorntukkertje 13:02e56051501a 491 }
bjorntukkertje 13:02e56051501a 492 if (afstand > 0.25 && pos >= 654 && pos < 682) {
bjorntukkertje 13:02e56051501a 493 // 240 graden
bjorntukkertje 11:aac90fca0290 494 MotorL1 = 1 ;
bjorntukkertje 13:02e56051501a 495 MotorL2 = 0 ;
bjorntukkertje 13:02e56051501a 496 MotorR3 = 0.40 ;
bjorntukkertje 13:02e56051501a 497 MotorR4 = 0 ;
bjorntukkertje 13:02e56051501a 498 wait_ms (1000);
bjorntukkertje 13:02e56051501a 499 }
bjorntukkertje 13:02e56051501a 500 if (afstand > 0.25 && pos >= 682 && pos < 710) {
bjorntukkertje 13:02e56051501a 501 // 250 graden
bjorntukkertje 11:aac90fca0290 502 MotorL1 = 1 ;
bjorntukkertje 13:02e56051501a 503 MotorL2 = 0 ;
bjorntukkertje 13:02e56051501a 504 MotorR3 = 0.45 ;
bjorntukkertje 13:02e56051501a 505 MotorR4 = 0 ;
bjorntukkertje 13:02e56051501a 506 wait_ms (1000);
bjorntukkertje 13:02e56051501a 507 }
bjorntukkertje 13:02e56051501a 508 if (afstand > 0.25 && pos >= 710 && pos < 737) {
bjorntukkertje 13:02e56051501a 509 // 260 graden
bjorntukkertje 11:aac90fca0290 510 MotorL1 = 1 ;
bjorntukkertje 13:02e56051501a 511 MotorL2 = 0 ;
bjorntukkertje 13:02e56051501a 512 MotorR3 = 0.50 ;
bjorntukkertje 13:02e56051501a 513 MotorR4 = 0 ;
bjorntukkertje 13:02e56051501a 514 wait_ms (1000);
bjorntukkertje 13:02e56051501a 515 }
bjorntukkertje 13:02e56051501a 516 if (afstand > 0.25 && pos >= 737 && pos < 765) {
bjorntukkertje 13:02e56051501a 517 // 270 graden
bjorntukkertje 11:aac90fca0290 518 MotorL1 = 1 ;
bjorntukkertje 13:02e56051501a 519 MotorL2 = 0 ;
bjorntukkertje 13:02e56051501a 520 MotorR3 = 0.55 ;
bjorntukkertje 13:02e56051501a 521 MotorR4 = 0 ;
bjorntukkertje 13:02e56051501a 522 wait_ms (1000);
bjorntukkertje 13:02e56051501a 523 }
bjorntukkertje 13:02e56051501a 524 if (afstand > 0.25 && pos >= 765 && pos < 793) {
bjorntukkertje 13:02e56051501a 525 // 280 graden
bjorntukkertje 11:aac90fca0290 526 MotorL1 = 1 ;
bjorntukkertje 13:02e56051501a 527 MotorL2 = 0 ;
bjorntukkertje 13:02e56051501a 528 MotorR3 = 0.60 ;
bjorntukkertje 13:02e56051501a 529 MotorR4 = 0 ;
bjorntukkertje 13:02e56051501a 530 wait_ms (1000);
bjorntukkertje 13:02e56051501a 531 }
bjorntukkertje 13:02e56051501a 532 if (afstand > 0.25 && pos >= 793 && pos < 820) {
bjorntukkertje 13:02e56051501a 533 // 290 graden
bjorntukkertje 11:aac90fca0290 534 MotorL1 = 1 ;
bjorntukkertje 13:02e56051501a 535 MotorL2 = 0 ;
bjorntukkertje 13:02e56051501a 536 MotorR3 = 0.65 ;
bjorntukkertje 13:02e56051501a 537 MotorR4 = 0 ;
bjorntukkertje 13:02e56051501a 538 wait_ms (1000);
bjorntukkertje 13:02e56051501a 539 }
bjorntukkertje 13:02e56051501a 540 if (afstand > 0.25 && pos >= 820 && pos < 848) {
bjorntukkertje 13:02e56051501a 541 // 300 graden
bjorntukkertje 11:aac90fca0290 542 MotorL1 = 1 ;
bjorntukkertje 13:02e56051501a 543 MotorL2 = 0 ;
bjorntukkertje 13:02e56051501a 544 MotorR3 = 0.70 ;
bjorntukkertje 13:02e56051501a 545 MotorR4 = 0 ;
bjorntukkertje 13:02e56051501a 546 wait_ms (1000);
bjorntukkertje 13:02e56051501a 547 }
bjorntukkertje 13:02e56051501a 548 if (afstand > 0.25 && pos >= 848 && pos < 876) {
bjorntukkertje 13:02e56051501a 549 // 310 graden
bjorntukkertje 11:aac90fca0290 550 MotorL1 = 1 ;
bjorntukkertje 13:02e56051501a 551 MotorL2 = 0 ;
bjorntukkertje 13:02e56051501a 552 MotorR3 = 0.75 ;
bjorntukkertje 13:02e56051501a 553 MotorR4 = 0 ;
bjorntukkertje 13:02e56051501a 554 wait_ms (1000);
bjorntukkertje 13:02e56051501a 555 }
bjorntukkertje 13:02e56051501a 556 if (afstand > 0.25 && pos >= 876 && pos < 903) {
bjorntukkertje 13:02e56051501a 557 // 320 graden
bjorntukkertje 11:aac90fca0290 558 MotorL1 = 1 ;
bjorntukkertje 13:02e56051501a 559 MotorL2 = 0 ;
bjorntukkertje 13:02e56051501a 560 MotorR3 = 0.80 ;
bjorntukkertje 13:02e56051501a 561 MotorR4 = 0 ;
bjorntukkertje 13:02e56051501a 562 wait_ms (1000);
bjorntukkertje 13:02e56051501a 563 }
bjorntukkertje 13:02e56051501a 564 if (afstand > 0.25 && pos >= 903 && pos < 931) {
bjorntukkertje 13:02e56051501a 565 // 330 graden
bjorntukkertje 11:aac90fca0290 566 MotorL1 = 1 ;
bjorntukkertje 13:02e56051501a 567 MotorL2 = 0 ;
bjorntukkertje 13:02e56051501a 568 MotorR3 = 0.85 ;
bjorntukkertje 13:02e56051501a 569 MotorR4 = 0 ;
bjorntukkertje 13:02e56051501a 570 wait_ms (1000);
bjorntukkertje 13:02e56051501a 571 }
bjorntukkertje 13:02e56051501a 572 if (afstand > 0.25 && pos >= 931 && pos < 956) {
bjorntukkertje 13:02e56051501a 573 // 340 graden
bjorntukkertje 11:aac90fca0290 574 MotorL1 = 1 ;
bjorntukkertje 13:02e56051501a 575 MotorL2 = 0 ;
bjorntukkertje 13:02e56051501a 576 MotorR3 = 0.90 ;
bjorntukkertje 13:02e56051501a 577 MotorR4 = 0 ;
bjorntukkertje 13:02e56051501a 578 wait_ms (1000);
bjorntukkertje 13:02e56051501a 579 }
bjorntukkertje 13:02e56051501a 580 if (afstand > 0.25 && pos >= 956 && pos < 987) {
bjorntukkertje 13:02e56051501a 581 // 350 graden
bjorntukkertje 11:aac90fca0290 582 MotorL1 = 1 ;
bjorntukkertje 13:02e56051501a 583 MotorL2 = 0 ;
bjorntukkertje 13:02e56051501a 584 MotorR3 = 0.95 ;
bjorntukkertje 13:02e56051501a 585 MotorR4 = 0 ;
bjorntukkertje 13:02e56051501a 586 wait_ms (1000);
bjorntukkertje 13:02e56051501a 587 }
bjorntukkertje 13:02e56051501a 588 }
bjorntukkertje 0:9756a10feb5f 589 }
joosthartkamp 16:a9382753e8d6 590 */