API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Feb 16th, 2019
80
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 23.16 KB | None | 0 0
raw download clone embed print report
  1. int a = 0;
  2. int q = 0;
  3. int t = 0;
  4. int b = 0;
  5. int c = 0;
  6. int d = 0;
  7. int z = 0;
  8. int p = 0;
  9. int g = 0;
  10. int i = 0;
  11. int s = 0;
  12. int r = 0;
  13. int f=0;
  14. int u=0;
  15. int e=0;
  16. const int leftbackward = 6;
  17. const int leftforward = 5;
  18. const int rightbackward = 10;
  19. const int rightforward = 9;
  20.  
  21. const int trigpin1 = 3; //linkssensor
  22. const int echopin1 = 2;
  23. const int trigpin2 = 8; //achtersensor
  24. const int echopin2 = 7;
  25. // const int trigpin3=11; //rechtssensor
  26. // const int echopin3=12;
  27. long tijd;
  28. long afstand;
  29.  
  30.  
  31. #include <Servo.h>
  32. int x;
  33. int detect;
  34.  
  35. Servo servo1;
  36. Servo servo2;
  37. Servo servo3;
  38.  
  39.  
  40. void links(int x) {
  41. servo2.write(x) ;
  42. }
  43.  
  44. void rechts(int x) {
  45. servo3.write(x) ;
  46. }
  47.  
  48.  
  49. void setup() {
  50.  
  51.  
  52.  
  53. pinMode(echopin1, INPUT);
  54. pinMode(trigpin1, OUTPUT);
  55. pinMode(echopin2, INPUT);
  56. pinMode(trigpin2, OUTPUT);
  57. // pinMode(echopin3, INPUT);
  58. // pinMode(trigpin3, OUTPUT);
  59.  
  60. pinMode(leftforward, OUTPUT);
  61. pinMode(leftbackward, OUTPUT);
  62. pinMode(rightforward, OUTPUT);
  63. pinMode(rightbackward, OUTPUT);
  64.  
  65. Serial.begin (9600);
  66.  
  67. servo1.attach (A5);
  68. servo2.attach (A4);
  69. servo3.attach (A3);
  70. servo1.write (80);
  71. servo2.write(110);
  72. servo3.write(35);
  73.  
  74.  
  75. }
  76.  
  77. long sensor(char x, char y)
  78. {
  79. digitalWrite(x, LOW);
  80. delay(5);
  81. digitalWrite(x, HIGH);
  82. delay(5);
  83. digitalWrite(x, LOW);
  84. tijd = pulseIn(y, HIGH);
  85. afstand = tijd / 58.2;
  86. return afstand;
  87. }
  88. void loop() {
  89.  
  90. Serial.println(t);
  91. long linkssensor = sensor(trigpin1, echopin1);
  92. // Serial.print("linkssensor: ");
  93. // Serial.println(linkssensor);
  94. long achtersensor = sensor(trigpin2, echopin2);
  95. // Serial.print("achtersensor: ");
  96. // Serial.println(achtersensor);
  97. // long achtersensor = sensor(trigpin3, echopin3);
  98. // Serial.print("achtersensor: ");
  99. // Serial.println(achtersensor);
  100.  
  101.  
  102.  
  103. if (t == 0) {
  104.  
  105. digitalWrite(leftbackward, 0);
  106. digitalWrite(leftforward, 210);
  107. digitalWrite(rightbackward, 0);
  108. digitalWrite(rightforward, 179);
  109.  
  110. if (linkssensor <= 9) {
  111. digitalWrite(leftbackward, 0);
  112. digitalWrite(leftforward, 0);
  113. digitalWrite(rightbackward, 0);
  114. digitalWrite(rightforward, 179);
  115.  
  116.  
  117. }
  118. if (linkssensor >= 13) {
  119. digitalWrite(leftbackward, 0);
  120. digitalWrite(leftforward, 210);
  121. digitalWrite(rightbackward, 0);
  122. digitalWrite(rightforward, 179);
  123.  
  124. }
  125.  
  126. }
  127. if (achtersensor >= 80 && achtersensor <= 90 && t == 0) {
  128. digitalWrite(5, 0);
  129. digitalWrite(6, 210);
  130. digitalWrite(9, 179);
  131. digitalWrite(10, 0);
  132. delay(1200);
  133. digitalWrite(5, 0);
  134. digitalWrite(6, 0);
  135. digitalWrite(9, 0);
  136. digitalWrite(10, 0);
  137.  
  138. links(x = 50);
  139. rechts(x = 110);
  140. delay(1000);
  141. servo1.write(30);
  142. delay(1000);
  143. links (x = 110);
  144. rechts (x = 35);
  145. delay(1000);
  146. t = 10;
  147.  
  148. if (t == 10) {
  149. digitalWrite(leftbackward, 0);
  150. digitalWrite(leftforward, 210);
  151. digitalWrite(rightbackward, 0);
  152. digitalWrite(rightforward, 179);
  153. delay(1000);
  154.  
  155. digitalWrite(5, 210);
  156. digitalWrite(6, 0);
  157. digitalWrite(9, 0);
  158. digitalWrite(10, 179);
  159. delay(1200);
  160. digitalWrite(5, 0);
  161. digitalWrite(6, 0);
  162. digitalWrite(9, 0);
  163. digitalWrite(10, 0);
  164. delay(1000);
  165. links(x = 50);
  166. rechts(x = 110);
  167. delay(1000);
  168. servo1.write(80);
  169. delay(1000);
  170. servo1.write(30);
  171. links(x = 110);
  172. rechts(x = 35);
  173. delay(1000);
  174. servo1.write(80);
  175. delay(1000);
  176. t = 11;
  177.  
  178. if (t == 11) {
  179. digitalWrite(5, 0);
  180. digitalWrite(6, HIGH);
  181. digitalWrite(9, HIGH);
  182. digitalWrite(10, 0);
  183. delay(2200);
  184. t = 2;
  185. }
  186. }
  187.  
  188. // einde
  189. t = 2;
  190. }
  191.  
  192.  
  193.  
  194. if (t == 1) {
  195.  
  196. digitalWrite(leftbackward, 0);
  197. digitalWrite(leftforward, 210);
  198. digitalWrite(rightbackward, 0);
  199. digitalWrite(rightforward, 179);
  200.  
  201. if (linkssensor <= 10) {
  202. digitalWrite(leftbackward, 0);
  203. digitalWrite(leftforward, 0);
  204. digitalWrite(rightbackward, 0);
  205. digitalWrite(rightforward, 179);
  206.  
  207.  
  208. }
  209. if (linkssensor >= 16) {
  210. digitalWrite(leftbackward, 0);
  211. digitalWrite(leftforward, 210);
  212. digitalWrite(rightbackward, 0);
  213. digitalWrite(rightforward, 0);
  214.  
  215. }
  216.  
  217. if (achtersensor >= 30 && achtersensor <= 45 && t == 1) {
  218. digitalWrite(5, 0);
  219. digitalWrite(6, 210);
  220. digitalWrite(9, 179);
  221. digitalWrite(10, 0);
  222. delay(1200);
  223. digitalWrite(5, 0);
  224. digitalWrite(6, 0);
  225. digitalWrite(9, 0);
  226. digitalWrite(10, 0);
  227. t = 2;
  228. }
  229. // ALS ER EEN VOORWERP STAAT OP PUNT 1A
  230. // if (achtersensor >= 20 && achtersensor <= 35 && t==1) {
  231. // digitalWrite(leftbackward, 0);
  232. // digitalWrite(leftforward, 0);
  233. // digitalWrite(rightbackward, 0);
  234. // digitalWrite(rightforward, 0);
  235. // links(x = 50);
  236. // rechts(x = 80);
  237. // delay(1000);
  238. // servo1.write(35);
  239. // delay(1000);
  240. // links (x = 120);
  241. // rechts (x = 30);
  242. // delay(1000);
  243. // t=2;
  244. //
  245. // }
  246.  
  247.  
  248. }
  249. if (t == 2) {
  250.  
  251.  
  252. digitalWrite(leftbackward, 0);
  253. digitalWrite(leftforward, 210);
  254. digitalWrite(rightbackward, 0);
  255. digitalWrite(rightforward, 179);
  256.  
  257. if (linkssensor <= 61) {
  258. digitalWrite(leftbackward, 0);
  259. digitalWrite(leftforward, 0);
  260. digitalWrite(rightbackward, 0);
  261. digitalWrite(rightforward, 179);
  262.  
  263.  
  264. }
  265. if (linkssensor >= 67) {
  266. digitalWrite(leftbackward, 0);
  267. digitalWrite(leftforward, 210);
  268. digitalWrite(rightbackward, 0);
  269. digitalWrite(rightforward, 0);
  270.  
  271. }
  272. g = g + 1;
  273. if (achtersensor >= 30 && achtersensor <= 40 && z == 0 && g >= 30) {
  274. z = 1;
  275. g = 0;
  276. digitalWrite(5, 0);
  277. digitalWrite(6, 0);
  278. digitalWrite(9, 0);
  279. digitalWrite(10, 0);
  280.  
  281. links(x = 50);
  282. rechts(x = 110);
  283. delay(1000);
  284. servo1.write(30);
  285. delay(1000);
  286. links (x = 110);
  287. rechts (x = 35);
  288. delay(1000);
  289. t = 12;
  290. if (t == 12) {
  291. digitalWrite(5, 0);
  292. digitalWrite(6, HIGH);
  293. digitalWrite(9, HIGH);
  294. digitalWrite(10, 0);
  295. delay(1800);
  296. digitalWrite(5, 0);
  297. digitalWrite(6, 0);
  298. digitalWrite(9, 0);
  299. digitalWrite(10, 0);
  300. delay(1000);
  301. t = 13;
  302. }
  303. }
  304. if (achtersensor >= 70 && achtersensor <= 80 && z == 1 && g >= 80) {
  305. z = 2;
  306. digitalWrite(5, 0);
  307. digitalWrite(6, 0);
  308. digitalWrite(9, 0);
  309. digitalWrite(10, 0);
  310.  
  311. links(x = 50);
  312. rechts(x = 110);
  313. delay(1000);
  314. servo1.write(30);
  315. delay(1000);
  316. links (x = 110);
  317. rechts (x = 35);
  318. delay(1000);
  319. t = 12;
  320. }
  321. }
  322. if (t == 12) {
  323. digitalWrite(5, 0);
  324. digitalWrite(6, HIGH);
  325. digitalWrite(9, HIGH);
  326. digitalWrite(10, 0);
  327. delay(1800);
  328. t = 15;
  329. }
  330.  
  331.  
  332.  
  333. if (t == 15) {
  334. digitalWrite(leftbackward, 0);
  335. digitalWrite(leftforward, 210);
  336. digitalWrite(rightbackward, 0);
  337. digitalWrite(rightforward, 179);
  338.  
  339. if (linkssensor <= 23) {
  340. digitalWrite(leftbackward, 0);
  341. digitalWrite(leftforward, 0);
  342. digitalWrite(rightbackward, 0);
  343. digitalWrite(rightforward, 179);
  344.  
  345.  
  346. }
  347. if (linkssensor >= 29) {
  348. digitalWrite(leftbackward, 0);
  349. digitalWrite(leftforward, 210);
  350. digitalWrite(rightbackward, 0);
  351. digitalWrite(rightforward, 0);
  352.  
  353. }
  354.  
  355.  
  356. if (achtersensor >= 80 && achtersensor <= 90) {
  357. digitalWrite(leftbackward, 0);
  358. digitalWrite(leftforward, 0);
  359. digitalWrite(rightbackward, 0);
  360. digitalWrite(rightforward, 0);
  361.  
  362. links(x = 50);
  363. rechts(x = 110);
  364. delay(1000);
  365. servo1.write(80);
  366. delay(1000);
  367. servo1.write(30);
  368. links(x = 110);
  369. rechts(x = 35);
  370. delay(1000);
  371. servo1.write(80);
  372. delay(1000);
  373. t = 16;
  374. }
  375. }
  376. if (t == 16) {
  377. digitalWrite(5, 0);
  378. digitalWrite(6, HIGH);
  379. digitalWrite(9, HIGH);
  380. digitalWrite(10, 0);
  381. delay(1200);
  382. t = 4;
  383. }
  384.  
  385.  
  386.  
  387.  
  388.  
  389.  
  390.  
  391. if (t == 13) {
  392. digitalWrite(leftbackward, 0);
  393. digitalWrite(leftforward, 210);
  394. digitalWrite(rightbackward, 0);
  395. digitalWrite(rightforward, 179);
  396.  
  397. if (linkssensor <= 24) {
  398. digitalWrite(leftbackward, 0);
  399. digitalWrite(leftforward, 0);
  400. digitalWrite(rightbackward, 0);
  401. digitalWrite(rightforward, 179);
  402.  
  403.  
  404. }
  405. if (linkssensor >= 28) {
  406. digitalWrite(leftbackward, 0);
  407. digitalWrite(leftforward, 210);
  408. digitalWrite(rightbackward, 0);
  409. digitalWrite(rightforward, 0);
  410.  
  411. }
  412.  
  413.  
  414. if (achtersensor >= 80 && achtersensor <= 90) {
  415. digitalWrite(leftbackward, 0);
  416. digitalWrite(leftforward, 0);
  417. digitalWrite(rightbackward, 0);
  418. digitalWrite(rightforward, 0);
  419.  
  420. links(x = 50);
  421. rechts(x = 110);
  422. delay(1000);
  423. servo1.write(80);
  424. delay(1000);
  425. servo1.write(30);
  426. links(x = 110);
  427. rechts(x = 35);
  428. delay(1000);
  429. servo1.write(80);
  430. delay(1000);
  431. t = 11;
  432. if (t == 11) {
  433. digitalWrite(5, 0);
  434. digitalWrite(6, HIGH);
  435. digitalWrite(9, HIGH);
  436. digitalWrite(10, 0);
  437. delay(2000);
  438. digitalWrite(5, 0);
  439. digitalWrite(6, 0);
  440. digitalWrite(9, 0);
  441. digitalWrite(10, 0);
  442. t = 17;
  443. }
  444. }
  445. }
  446. if (t == 17) {
  447. digitalWrite(leftbackward, 0);
  448. digitalWrite(leftforward, 210);
  449. digitalWrite(rightbackward, 0);
  450. digitalWrite(rightforward, 179);
  451.  
  452. if (linkssensor <= 61) {
  453. digitalWrite(leftbackward, 0);
  454. digitalWrite(leftforward, 0);
  455. digitalWrite(rightbackward, 0);
  456. digitalWrite(rightforward, 179);
  457.  
  458.  
  459. }
  460. if (linkssensor >= 67) {
  461. digitalWrite(leftbackward, 0);
  462. digitalWrite(leftforward, 210);
  463. digitalWrite(rightbackward, 0);
  464. digitalWrite(rightforward, 0);
  465.  
  466. }
  467. i = i + 1;
  468. if (achtersensor >= 60 && achtersensor <= 70 && i >= 70) {
  469.  
  470. digitalWrite(5, 0);
  471. digitalWrite(6, 0);
  472. digitalWrite(9, 0);
  473. digitalWrite(10, 0);
  474.  
  475. links(x = 50);
  476. rechts(x = 110);
  477. delay(1000);
  478. servo1.write(30);
  479. delay(1000);
  480. links (x = 110);
  481. rechts (x = 35);
  482. delay(1000);
  483. digitalWrite(5, 0);
  484. digitalWrite(6, HIGH);
  485. digitalWrite(9, HIGH);
  486. digitalWrite(10, 0);
  487. delay(1600);
  488. t = 18;
  489. }
  490. }
  491. if (t == 18) {
  492. digitalWrite(leftbackward, 0);
  493. digitalWrite(leftforward, 210);
  494. digitalWrite(rightbackward, 0);
  495. digitalWrite(rightforward, 179);
  496.  
  497. if (linkssensor <= 24) {
  498. digitalWrite(leftbackward, 0);
  499. digitalWrite(leftforward, 0);
  500. digitalWrite(rightbackward, 0);
  501. digitalWrite(rightforward, 179);
  502.  
  503.  
  504. }
  505. if (linkssensor >= 28) {
  506. digitalWrite(leftbackward, 0);
  507. digitalWrite(leftforward, 210);
  508. digitalWrite(rightbackward, 0);
  509. digitalWrite(rightforward, 0);
  510.  
  511. }
  512.  
  513. e=e+1;
  514. if (achtersensor >= 80 && achtersensor <= 90 && e>=50) {
  515. digitalWrite(leftbackward, 0);
  516. digitalWrite(leftforward, 0);
  517. digitalWrite(rightbackward, 0);
  518. digitalWrite(rightforward, 0);
  519.  
  520. links(x = 50);
  521. rechts(x = 110);
  522. delay(1000);
  523. servo1.write(80);
  524. delay(1000);
  525. servo1.write(30);
  526. links(x = 110);
  527. rechts(x = 35);
  528. delay(1000);
  529. servo1.write(80);
  530. delay(1000);
  531. digitalWrite(5, 0);
  532. digitalWrite(6, HIGH);
  533. digitalWrite(9, HIGH);
  534. digitalWrite(10, 0);
  535. delay(1200);
  536. t = 5;
  537. }
  538. }
  539.  
  540.  
  541.  
  542.  
  543.  
  544.  
  545.  
  546.  
  547.  
  548.  
  549.  
  550.  
  551.  
  552.  
  553.  
  554.  
  555.  
  556.  
  557.  
  558. // d = d + 1; //als 1a voorwerp is
  559. // if (achtersensor >= 80 && t == 2 && d >= 50 && z == 1) {
  560. // digitalWrite(leftbackward, 0);
  561. // digitalWrite(leftforward, 0);
  562. // digitalWrite(rightbackward, 0);
  563. // digitalWrite(rightforward, 0);
  564. // delay(100);
  565. //
  566. // digitalWrite(5, 0);
  567. // digitalWrite(6, 210);
  568. // digitalWrite(9, 179);
  569. // digitalWrite(10, 0);
  570. // delay(1800);
  571. // digitalWrite(5, 0);
  572. // digitalWrite(6, 0);
  573. // digitalWrite(9, 0);
  574. // digitalWrite(10, 0);
  575. // delay(1000);
  576. // t = 4;
  577. //
  578. // }
  579.  
  580.  
  581.  
  582.  
  583.  
  584.  
  585. if (t == 4) {
  586.  
  587. digitalWrite(leftbackward, 0);
  588. digitalWrite(leftforward, 210);
  589. digitalWrite(rightbackward, 0);
  590. digitalWrite(rightforward, 179);
  591.  
  592. if (linkssensor <= 23) {
  593. digitalWrite(leftbackward, 0);
  594. digitalWrite(leftforward, 0);
  595. digitalWrite(rightbackward, 0);
  596. digitalWrite(rightforward, 179);
  597.  
  598.  
  599. }
  600. if (linkssensor >= 29) {
  601. digitalWrite(leftbackward, 0);
  602. digitalWrite(leftforward, 210);
  603. digitalWrite(rightbackward, 0);
  604. digitalWrite(rightforward, 0);
  605.  
  606. }
  607. q = q + 1;
  608.  
  609. if (achtersensor >= 80 && achtersensor <= 90 && q >= 50) {
  610. digitalWrite(5, 0);
  611. digitalWrite(6, 210);
  612. digitalWrite(9, 179);
  613. digitalWrite(10, 0);
  614. delay(900);
  615. digitalWrite(5, 0);
  616. digitalWrite(6, 0);
  617. digitalWrite(9, 0);
  618. digitalWrite(10, 0);
  619. delay(1000);
  620. t = 5;
  621.  
  622. }
  623. }
  624.  
  625.  
  626. if (t == 5) {
  627.  
  628. digitalWrite(leftbackward, 0);
  629. digitalWrite(leftforward, 210);
  630. digitalWrite(rightbackward, 0);
  631. digitalWrite(rightforward, 179);
  632.  
  633. if (linkssensor <= 12) {
  634. digitalWrite(leftbackward, 0);
  635. digitalWrite(leftforward, 0);
  636. digitalWrite(rightbackward, 0);
  637. digitalWrite(rightforward, 179);
  638.  
  639.  
  640. }
  641. if (linkssensor >= 18) {
  642. digitalWrite(leftbackward, 0);
  643. digitalWrite(leftforward, 210);
  644. digitalWrite(rightbackward, 0);
  645. digitalWrite(rightforward, 0);
  646.  
  647. }
  648. a = a + 1;
  649.  
  650. if (achtersensor >= 35 && achtersensor <= 45 && a >= 50) {
  651. digitalWrite(5, 0);
  652. digitalWrite(6, 0);
  653. digitalWrite(9, 0);
  654. digitalWrite(10, 0);
  655.  
  656. links(x = 50);
  657. rechts(x = 110);
  658. delay(1000);
  659. servo1.write(30);
  660. delay(1000);
  661. links (x = 110);
  662. rechts (x = 35);
  663. delay(1000);
  664. t = 18;
  665. }
  666. }
  667. if ( t == 18 ) {
  668.  
  669. digitalWrite(leftbackward, 0);
  670. digitalWrite(leftforward, 210);
  671. digitalWrite(rightbackward, 0);
  672. digitalWrite(rightforward, 179);
  673.  
  674. if (linkssensor <= 12) {
  675. digitalWrite(leftbackward, 0);
  676. digitalWrite(leftforward, 0);
  677. digitalWrite(rightbackward, 0);
  678. digitalWrite(rightforward, 179);
  679.  
  680.  
  681. }
  682. if (linkssensor >= 18) {
  683. digitalWrite(leftbackward, 0);
  684. digitalWrite(leftforward, 210);
  685. digitalWrite(rightbackward, 0);
  686. digitalWrite(rightforward, 0);
  687.  
  688. }
  689. s = s + 1;
  690. if (achtersensor >= 57 && achtersensor <= 63 && s >= 30) {
  691. digitalWrite(5, 210);
  692. digitalWrite(6, 0);
  693. digitalWrite(9, 0);
  694. digitalWrite(10, 179);
  695. delay(1100);
  696. digitalWrite(5, 0);
  697. digitalWrite(6, 0);
  698. digitalWrite(9, 0);
  699. digitalWrite(10, 0);
  700. delay(1000);
  701. links(x = 50);
  702. rechts(x = 110);
  703. delay(1000);
  704. servo1.write(80);
  705. delay(1000);
  706. servo1.write(30);
  707. links(x = 110);
  708. rechts(x = 35);
  709. delay(1000);
  710. servo1.write(80);
  711. delay(1000);
  712. t = 20;
  713. }
  714. }
  715. if (t == 20) {
  716. digitalWrite(5, 0);
  717. digitalWrite(6, HIGH);
  718. digitalWrite(9, HIGH);
  719. digitalWrite(10, 0);
  720. delay(2000);
  721. t = 6;
  722. }
  723.  
  724.  
  725.  
  726.  
  727.  
  728.  
  729.  
  730.  
  731.  
  732.  
  733.  
  734.  
  735.  
  736. if (t == 6) {
  737.  
  738. digitalWrite(leftbackward, 0);
  739. digitalWrite(leftforward, 210);
  740. digitalWrite(rightbackward, 0);
  741. digitalWrite(rightforward, 179);
  742.  
  743. if (linkssensor <= 30) {
  744. digitalWrite(leftbackward, 0);
  745. digitalWrite(leftforward, 0);
  746. digitalWrite(rightbackward, 0);
  747. digitalWrite(rightforward, 179);
  748.  
  749.  
  750. }
  751. if (linkssensor >= 36) {
  752. digitalWrite(leftbackward, 0);
  753. digitalWrite(leftforward, 210);
  754. digitalWrite(rightbackward, 0);
  755. digitalWrite(rightforward, 0);
  756.  
  757. }
  758. b = b + 1;
  759.  
  760. if (achtersensor >= 30 && achtersensor <= 40 && b >= 50) {
  761. digitalWrite(leftbackward, 0);
  762. digitalWrite(leftforward, 0);
  763. digitalWrite(rightbackward, 0);
  764. digitalWrite(rightforward, 0);
  765. delay(1000);
  766. links(x = 50);
  767. rechts(x = 110);
  768. delay(1000);
  769. servo1.write(30);
  770. delay(1000);
  771. links (x = 110);
  772. rechts (x = 35);
  773. delay(1000);
  774. t = 21;
  775. }
  776. }
  777. if (t == 21) {
  778. digitalWrite(5, 0);
  779. digitalWrite(6, HIGH);
  780. digitalWrite(9, HIGH);
  781. digitalWrite(10, 0);
  782. delay(1800);
  783. t = 7;
  784. }
  785.  
  786.  
  787. if (t == 7) {
  788.  
  789. digitalWrite(leftbackward, 0);
  790. digitalWrite(leftforward, 210);
  791. digitalWrite(rightbackward, 0);
  792. digitalWrite(rightforward, 179);
  793.  
  794. if (linkssensor <= 51) {
  795. digitalWrite(leftbackward, 0);
  796. digitalWrite(leftforward, 0);
  797. digitalWrite(rightbackward, 0);
  798. digitalWrite(rightforward, 179);
  799.  
  800.  
  801. }
  802. if (linkssensor >= 57) {
  803. digitalWrite(leftbackward, 0);
  804. digitalWrite(leftforward, 210);
  805. digitalWrite(rightbackward, 0);
  806. digitalWrite(rightforward, 0);
  807.  
  808. }
  809. r = r + 1;
  810. if (achtersensor >= 80 && achtersensor <= 90 && r >= 30) {
  811. digitalWrite(leftbackward, 0);
  812. digitalWrite(leftforward, 0);
  813. digitalWrite(rightbackward, 0);
  814. digitalWrite(rightforward, 0);
  815. delay(1000);
  816. links(x = 50);
  817. rechts(x = 110);
  818. delay(1000);
  819. servo1.write(80);
  820. delay(1000);
  821. servo1.write(30);
  822. links(x = 110);
  823. rechts(x = 35);
  824. delay(1000);
  825. servo1.write(80);
  826. delay(1000);
  827. t = 22;
  828. }
  829. }
  830. if (t == 22) {
  831. digitalWrite(5, 0);
  832. digitalWrite(6, HIGH);
  833. digitalWrite(9, HIGH);
  834. digitalWrite(10, 0);
  835. delay(1900);
  836. t = 23;
  837. }
  838. if (t == 23) {
  839. digitalWrite(leftbackward, 0);
  840. digitalWrite(leftforward, 210);
  841. digitalWrite(rightbackward, 0);
  842. digitalWrite(rightforward, 179);
  843.  
  844. if (linkssensor <= 30) {
  845. digitalWrite(leftbackward, 0);
  846. digitalWrite(leftforward, 0);
  847. digitalWrite(rightbackward, 0);
  848. digitalWrite(rightforward, 179);
  849.  
  850.  
  851. }
  852. if (linkssensor >= 36) {
  853. digitalWrite(leftbackward, 0);
  854. digitalWrite(leftforward, 210);
  855. digitalWrite(rightbackward, 0);
  856. digitalWrite(rightforward, 0);
  857.  
  858. }
  859. f=f+1;
  860. if (achtersensor>=60 && achtersensor<=70 && f>=50){
  861. digitalWrite(leftbackward, 0);
  862. digitalWrite(leftforward, 0);
  863. digitalWrite(rightbackward, 0);
  864. digitalWrite(rightforward, 0);
  865. delay(1000);
  866. links(x = 50);
  867. rechts(x = 110);
  868. delay(1000);
  869. servo1.write(30);
  870. delay(1000);
  871. links (x = 110);
  872. rechts (x = 35);
  873. delay(1000);
  874. t=24;
  875. }}
  876. if (t == 24){
  877. digitalWrite(5, 0);
  878. digitalWrite(6, HIGH);
  879. digitalWrite(9, HIGH);
  880. digitalWrite(10, 0);
  881. delay(1800);
  882. t = 25;
  883. }
  884.  
  885. if(t==25){
  886. digitalWrite(leftbackward, 0);
  887. digitalWrite(leftforward, 210);
  888. digitalWrite(rightbackward, 0);
  889. digitalWrite(rightforward, 179);
  890.  
  891. if (linkssensor <= 51) {
  892. digitalWrite(leftbackward, 0);
  893. digitalWrite(leftforward, 0);
  894. digitalWrite(rightbackward, 0);
  895. digitalWrite(rightforward, 179);
  896.  
  897.  
  898. }
  899. if (linkssensor >= 57) {
  900. digitalWrite(leftbackward, 0);
  901. digitalWrite(leftforward, 210);
  902. digitalWrite(rightbackward, 0);
  903. digitalWrite(rightforward, 0);
  904.  
  905. }
  906. u=u+1;
  907. if (achtersensor >= 80 && achtersensor <= 90 && u >= 30) {
  908. digitalWrite(leftbackward, 0);
  909. digitalWrite(leftforward, 0);
  910. digitalWrite(rightbackward, 0);
  911. digitalWrite(rightforward, 0);
  912. delay(1000);
  913. links(x = 50);
  914. rechts(x = 110);
  915. delay(1000);
  916. servo1.write(80);
  917. delay(1000);
  918. servo1.write(30);
  919. links(x = 110);
  920. rechts(x = 35);
  921. delay(1000);
  922. servo1.write(80);
  923. delay(1000);
  924. digitalWrite(leftbackward, 0);
  925. digitalWrite(leftforward, 0);
  926. digitalWrite(rightbackward, 0);
  927. digitalWrite(rightforward, 0);
  928.  
  929.  
  930.  
  931.  
  932.  
  933.  
  934. }
  935.  
  936. }
  937.  
  938.  
  939. }
  940.  
  941.  
  942.  
  943.  
  944.  
  945.  
  946. // ALS ER IETS STAAT OP PUNT 1B
  947. // if (achtersensor>=35 && t==2){
  948. // digitalWrite(leftbackward, 0);
  949. // digitalWrite(leftforward, 0);
  950. // digitalWrite(rightbackward, 0);
  951. // digitalWrite(rightforward, 0);
  952. // links(x = 50);
  953. // rechts(x = 80);
  954. // delay(1000);
  955. // servo1.write(35);
  956. // delay(1000);
  957. // links (x = 120);
  958. // rechts (x = 30);
  959. // delay(1000);
  960. // }
  961.  
  962. // ALS ER IETS STAAT OP PUNT 1C
  963. // if (achtersensor>=60 && t==2){
  964. // digitalWrite(leftbackward, 0);
  965. // digitalWrite(leftforward, 0);
  966. // digitalWrite(rightbackward, 0);
  967. // digitalWrite(rightforward, 0);
  968. // links(x = 50);
  969. // rechts(x = 80);
  970. // delay(1000);
  971. // servo1.write(35);
  972. // delay(1000);
  973. // links (x = 120);
  974. // rechts (x = 30);
  975. // delay(1000);
  976. // }
  977.  
  978.  
  979.  
  980.  
  981.  
  982.  
  983. // if (t==3){
  984. //
  985. // digitalWrite(5, 190);
  986. // digitalWrite(6, 0);
  987. // digitalWrite(9, 0);
  988. // digitalWrite(10, 162);
  989. // delay(450);
  990. // links(x=50);
  991. // rechts(x=80);
  992. // delay(1000);
  993. // servo1.write(90);
  994. // delay(1000);
  995. //
  996. // links (x=120);
  997. // rechts(x=30);
  998. //
  999. // digitalWrite(5, 0);
  1000. // digitalWrite(6, 190);
  1001. // digitalWrite(9, 162);
  1002. // digitalWrite(10, 0);
  1003. // delay(450);
  1004. //
  1005. // t=4;
  1006. // }
  1007. // if (t==4){
  1008. // digitalWrite(leftbackward, 0);
  1009. // digitalWrite(leftforward, 190);
  1010. // digitalWrite(rightbackward, 0);
  1011. // digitalWrite(rightforward, 162);
  1012. //
  1013. // if (linkssensor <= 10) {
  1014. // digitalWrite(leftbackward, 0);
  1015. // digitalWrite(leftforward, 0);
  1016. // digitalWrite(rightbackward, 0);
  1017. // digitalWrite(rightforward, 162);
  1018. //
  1019. //
  1020. // }
  1021. // if (linkssensor >= 18) {
  1022. // digitalWrite(leftbackward, 0);
  1023. // digitalWrite(leftforward, 190);
  1024. // digitalWrite(rightbackward, 0);
  1025. // digitalWrite(rightforward, 0);
  1026. //
  1027. // }
  1028. //
  1029. // if(achtersensor>=50 && t==4){
  1030. // digitalWrite(5, 0);
  1031. // digitalWrite(6, 190);
  1032. // digitalWrite(9, 162);
  1033. // digitalWrite(10, 0);
  1034. // delay(900);
  1035. // digitalWrite(5, 0);
  1036. // digitalWrite(6, 0);
  1037. // digitalWrite(9, 0);
  1038. // digitalWrite(10, 0);
  1039. // t=5;
  1040. //
  1041. // }}
  1042. //
  1043. // if (t==5){
  1044. //
  1045. // digitalWrite(leftbackward, 0);
  1046. // digitalWrite(leftforward, 190);
  1047. // digitalWrite(rightbackward, 0);
  1048. // digitalWrite(rightforward, 162);
  1049. //
  1050. // if (linkssensor <= 10) {
  1051. // digitalWrite(leftbackward, 0);
  1052. // digitalWrite(leftforward, 0);
  1053. // digitalWrite(rightbackward, 0);
  1054. // digitalWrite(rightforward, 162);
  1055. //
  1056. //
  1057. // }
  1058. // if (linkssensor >= 18) {
  1059. // digitalWrite(leftbackward, 0);
  1060. // digitalWrite(leftforward, 190);
  1061. // digitalWrite(rightbackward, 0);
  1062. // digitalWrite(rightforward, 0);
  1063. //
  1064. // }
  1065. //
  1066. // }
  1067. //}
  1068. //
  1069. ////
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!