tdulik

Uklizec - Franta & Franta

Nov 20th, 2018
125
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. var HLEDEJ = 1;
  2. var ROVNE = 2;
  3. var CUKNI = 3;
  4. var ZPATECKA = 4;
  5. var OTOC180 = 5;
  6. var STOP = 6;
  7. var ZPET_PRI_ZPATECCE = 7;
  8. var OTOC_PRI_ZPATECCE = 8;
  9. var caraL = 0;
  10. var caraR = 0;
  11. var stav = STOP;
  12. var vzdalenost = 0;
  13. var dist = 0;
  14. var casovac = 1;
  15. var ultrazvuk;
  16. var stopky = 0;
  17. var minuleX=0;
  18. function HCSRcb(x) {
  19.     if (x<300) {
  20.         vzdalenost = (x + minuleX) / 2;
  21.         minuleX = x;
  22.     }
  23. }
  24.  
  25. ultrazvuk = (require("HC-SR04").connect(B11, B10, HCSRcb)); //PORT_IN3
  26.  
  27. function spustiDopravnik() {
  28.     //pasovy dopravnik:
  29.     analogWrite(E13, 0.05, {
  30.         freq: 50
  31.     });
  32.     analogWrite(E14, 0.1, {
  33.         freq: 50
  34.     });
  35.     //vyklopka vodorovne - do klidoveho stavu :
  36.     analogWrite(B14, 0.09, {
  37.         freq: 50
  38.     });
  39.    
  40. }
  41. function zastavDopravnik() {
  42.     analogWrite(E13, 0.075, {
  43.         freq: 50
  44.     });
  45.     analogWrite(E14, 0.075, {
  46.         freq: 50
  47.     });
  48. }
  49.  
  50. function vyklop() {
  51.     print('Vyklapim..');
  52.     analogWrite(B14, 0.05, {
  53.         freq: 50
  54.     });
  55. }
  56. function rovne() {
  57.     print('jedu rovne');
  58.     analogWrite(E9, 0.05, {
  59.         freq: 50
  60.     });
  61.     analogWrite(E11, 0.1, {
  62.         freq: 50
  63.     });
  64. }
  65. function zpet() {
  66.     print('jedu zpet');
  67.     analogWrite(E9, 0.1, {
  68.         freq: 50
  69.     });
  70.     analogWrite(E11, 0.05, {
  71.         freq: 50
  72.     });
  73. }
  74. function otacejL() {
  75.     print('otacim vlevo');
  76.     analogWrite(E9, 0.05, {
  77.         freq: 50
  78.     });
  79.     analogWrite(E11, 0.05, {
  80.         freq: 50
  81.     });
  82. }
  83. function skenuj() {
  84.     print('Skenuji-hledam krabici');
  85.     analogWrite(E9, 0.072, {
  86.         freq: 50
  87.     });
  88.     analogWrite(E11, 0.072, {
  89.         freq: 50
  90.     });
  91. }
  92.  
  93. function otacejR() {
  94.     print('otacim vpravo');
  95.     analogWrite(E9, 0.1, {
  96.         freq: 50
  97.     });
  98.     analogWrite(E11, 0.1, {
  99.         freq: 50
  100.     });
  101. }
  102. function stojim() {
  103.     print('stojim..');
  104.     analogWrite(E9, 0.074, {
  105.         freq: 50
  106.     });
  107.     analogWrite(E11, 0.074, {
  108.         freq: 50
  109.     });
  110.     //vyklopka:
  111.     analogWrite(B14, 0.05, {
  112.         freq: 50
  113.     });
  114.     zastavDopravnik();
  115. }
  116.  
  117. setWatch(function () {
  118.     if (stav == STOP) {
  119.         stav = HLEDEJ;
  120.         stopky = 0;
  121.         casovac=0;
  122.         rovne();
  123.         spustiDopravnik();
  124.         digitalWrite(D9, false); //RED1 led
  125.     } else {
  126.         stav = STOP;
  127.         stojim();
  128.     }
  129. }, "C5", {
  130.     repeat: true,
  131.     edge: 'rising',
  132.     debounce: 100
  133. });
  134.  
  135. setInterval(function () {
  136.     caraL = digitalRead(C12) == 0; // PORT_IN1
  137.     caraR = digitalRead(B7) == 0; // PORT_IN2
  138.     caraBack = digitalRead(C10) === 0; // && digitalRead(C11) === 0; // nefunguje  
  139.     ultrazvuk.trigger();
  140.     //print(stav);
  141.     //print(vzdalenost);
  142.     //print(caraL);
  143.     //print(caraR);
  144.     casovac++;
  145.     stopky++;
  146.     if (stav != STOP) {
  147.         //print("Stav=" + stav + " casovac="+casovac+" stopky=" + stopky);
  148.         if (stopky == 100) {    // po minute a pul zacni hledat krabici...
  149.             digitalWrite(D9, true);
  150.             stav = ZPATECKA;
  151.             zastavDopravnik();
  152.             otacejL();
  153.         }
  154.     }
  155.     switch (stav) {
  156.     case HLEDEJ:
  157.         if (caraL) {
  158.             otacejR();
  159.             stav = OTOC180;
  160.             casovac = 0;
  161.         } else if (caraR) {
  162.             otacejL();
  163.             stav = OTOC180;
  164.             casovac = 0;
  165.         }
  166.         break;
  167.     case OTOC180:
  168.         if (casovac == 10) {
  169.             stav = HLEDEJ;
  170.             rovne();
  171.             casovac = 0;
  172.         }
  173.         break;
  174.     case ZPATECKA:
  175.         if (caraBack) {
  176.             rovne();
  177.             stav = ZPET_PRI_ZPATECCE;
  178.             casovac = 0;           
  179.         } else if (vzdalenost < 10) {
  180.             stav = STOP;
  181.             stojim();
  182.             vyklop();
  183.         } else if (vzdalenost < 150) {         
  184.             zpet();
  185.             casovac = 0;           
  186.         } else {
  187.             skenuj();
  188.             if (casovac > 50) {
  189.                 casovac = 0;
  190.                 zpet();
  191.             }
  192.         }
  193.         break;
  194.     case ZPET_PRI_ZPATECCE:
  195.         if (casovac == 20) {
  196.             stav=OTOC_PRI_ZPATECCE;
  197.             otacejR();
  198.             casovac = 0;
  199.         }
  200.         break;
  201.     case OTOC_PRI_ZPATECCE:
  202.         if (casovac == 20) {
  203.             stav = ZPATECKA;
  204.             zpet();
  205.             casovac = 0;
  206.         }  
  207.         break;
  208.     }
  209. }, 100);
RAW Paste Data