project enzo / Mbed 2 deprecated robot Featured

dit is em

Dependencies:   mbed

main.cpp@17:9bbd95969b6a, 2017-05-17 (annotated)

Committer:
krullie666
Date:
Wed May 17 09:54:57 2017 +0000
Revision:
17:9bbd95969b6a
Parent:
16:a9382753e8d6 
pinnen veranderd

Who changed what in which revision?

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