Arduino autonomní roboauto


Ukázkový kód pro jednoduché roboautíčko, robotické auto, autonomní elektroauto ovládané malým Arduinem NANO.

Obyčejný, levný 8bit procesor ATmega328P je hlavním řídím prvkem.  Dokáže vyhodnocovat dvě UTRAZVUKOVÉ čidla vzdálenosti, jedno LASEROVÉ čidlo, ovládat čtyři SERVO motory, ovládat signalizační LED světla a vše zobrazovat na OLED displeji.

Napájení je z 5V 2A powerbanky (přímo na 5V větev Arduina). Jedno UZ čidlo detekuje překážky vepředu, druhé vzadu. Infračervený laser je v podvozku a směřuje dolů aby sledoval to, jestli je autíčko na zemi nebo zvednuté. Na OLED displeji se zobrazují hodnoty čidel a provozní módy pro snadnější ladění kódu a odhalování chyb. Také jsou připojeny signalizační LED (samozřejmě přes patřičný rezistor).

Celý princip je jednoduchý a určen pro zábavu dětem (pod dozorem), případně výuku programování. Bez jakéhokoliv ovládacího tlačítka. Jen za pomoci USB se připojí powerbanka jako zdroj napájení (z USB PB na 5V větev Arduina). Následně autíčko setrvává v klidu, ale už proběhne start periferií. Po zvednutí a opětovném položení se uvede do pohybu vpřed. Jelikož PWM regulace pohybu servomotoru snižuje jak rychlost tak i okamžitý výkon, nelze motory příliš zpomalit. Jízda po koberci by byla nepřekonatelná. Proto je navolena střední rychlost a jízda je přerušovaná. Autíčko může náhodně zastavit a změnit směr. Po detekci překážky se zastaví, couvne, otočí apod. Když je autíčko zvednuto, zastaví se. Po položení na zem stále stojí (aby se nerozjelo např. při přemístění na stůl). Pro dalším zvednutí a položení se rozjede (a tak stále dokola). Je třeba brát v potaz, že chování závisí na kvalitě použitých čidel. UZ čidla mají užší detekční záběr než je šířka autíčka, proto není detekce dokonalá.

Vylepšit autíčko by bylo možné mnoha způsoby. Např. pokud se snaží pohybovat vpřed ale vzdálenost se nemění, asi se zaseklo a je třeba se otočit. Také by se dalo oželet třeba zapojení jedné LED nebo přípravy na zvuk a s použitím proudového senzoru sledovat zatížení motorků. Pro více čidel, otáčení čidel, ovládání přes BT, hodiny RTC, sledování GPS, logování a trasování na SD kartu, ovládání přes Android, …, by bylo třeba použít už větší, výkonnější a dalšími vstupy / výstupy vybavenější Arduino MEGA s procesorem ATmega2560.


Níže je základní funkční (neodladěný) kód (C++) pro inspiraci:

////////////////////////////////////////////////////////////////////////////////////////////////////////
// AUTONOMNI elektro ROBOAUTO s Arduino nano (neodladena verze)
// Pohon 4 motory, otaceni smykem, OLED stavovy displej, napajeni z powerbanky 5V/2A mimo USB
// Predni a zadni utrazvukovy senzor vzdalenosti, spodni infra-laserovy senzor vzdalenosti
// Predni svetla trvale sviti, ovladana zadni cervena svetla a blinkry
// Zapojene repro, nenaprogramovane
// Verze s nekontinualnim pomalym popojizdenim a nahodnym otacenim
// Snazi se vyhnout prekazkam, otacet se, couvat
// Po privedeni napajeni stoji, po zvednuti a polozeni jede, po zvednuti a polozeni stoji a tak dokola.
////////////////////////////////////////////////////////////////////////////////////////////////////////

////////////////////////////////////////////////////////////////////////////////////////////////////////
// Knihovny, zapojeni a promenne pro dane zapojeni ///////////////////////

#include <SPI.h>               //Komunikace SPI pro OLED
#include <Adafruit_GFX.h>      //OLED
#include <Adafruit_SSD1306.h>  //OLED
#include <Servo.h>             //Servo

// Nastaveni displeje //////////////////////////////////////////////

#define SCREEN_WIDTH 128  //Sirka OLED displeje
#define SCREEN_HEIGHT 64  //Vyska OLED displeje

#define OLED_MOSI 3    //Pripojeni OLED DI (PIN)
#define OLED_CLK 2     //Pripojeni OLED D0 (PIN)
#define OLED_DC 6      //Pripojeni OLED DC (PIN)
#define OLED_CS 7      //Pripojeni OLED CS (PIN)
#define OLED_RESET 12  //Pripojeni OLED RST (PIN)

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT,
                         OLED_MOSI, OLED_CLK, OLED_DC, OLED_RESET, OLED_CS);

// Ultrazvukove senzory vzdalenosti //////////////////////////////////

int UZ_PRED_VYSILAC = 13;                    //UZ meric vzdalenosti - vysilac predni (PIN)
int UZ_PRED_PRIJIMAC = 17;                   //UZ meric vzdalenosti - prijimac predni (PIN)
int UZ_ZAD_VYSILAC = 14;                     //UZ meric vzdalenosti - vysilac zadni (PIN)
int UZ_ZAD_PRIJIMAC = 15;                    //UZ meric vzdalenosti - prijimac zadni (PIN)
long UZ_PRED_ODEZVA, UZ_PRED_VZDALENOST, UZ_ZAD_ODEZVA, UZ_ZAD_VZDALENOST;  //Promenne pro mereni UZ
int M_UZ_PREDNI = 0;                                                        //UZ predni vzdalenost cm
int M_UZ_ZADNI = 0;                                                         //UZ zadni vzdalenost cm

// Laserove mereni vzdalenosti //////////////////////////////////////////////

int LASER = 19;         //Lidar (infra) (PIN)
int M_LASER_DOLNI = 0;  //Vzdalenost lidar mm
unsigned long LASER_TRVANI;

// Pohon, ovladani driveru servomotoru ///////////////////////

#define MOTOR_PA 9   // Driver motoru pravy (PIN)
#define MOTOR_PB 10  // Driver motoru pravy (PIN)
#define MOTOR_LA 11  // Driver motoru levy (PIN)
#define MOTOR_LB 5   // Driver motoru levy (PIN)

byte MOTOR_P_VPRED = 0;  //vychozi rychlost pohonneho motoru
byte MOTOR_P_ZPET = 0;   //vychozi rychlost pohonneho motoru
byte MOTOR_L_VPRED = 0;  //vychozi rychlost pohonneho motoru
byte MOTOR_L_ZPET = 0;   //vychozi rychlost pohonneho motoru

// Svetla, zvuky ///////////////////////////////////////////////////

int PIEZO = 16;        //Pripojeny reproduktor (PIN)
int BLINKR_PRAVY = 4;  //Připojene blinkry (PIN)
int BLINKR_LEVY = 8;   //Připojene blinkry (PIN)
int ZPATECKA = 1;      //Připojene zadni svetla (PIN)

// Dalsi promenne //////////////////////////////////////////////

byte MOD_A = 0;
byte MOD_B = 0;
byte MOD_M = 0;
byte BOOT = 0;
byte POCET_COUVANI = 0;
unsigned long MOD_M_CAS_RUN;
unsigned long MOD_M_CAS_STOP;

unsigned long BLINKR_CAS;
byte BLINKR;
unsigned long MOTOR_CAS;

////////////////////////////////////////////////////////////////////////////////////////////////////////
// Uvodni nastaveni, setup pri startu //////////////////////////////////////////////

void setup() {

  display.begin();         // Start displeje
  display.clearDisplay();  //Vymazani pameti displeje

  // Vstupy, vystupy

  pinMode(UZ_PRED_VYSILAC, OUTPUT);
  pinMode(UZ_PRED_PRIJIMAC, INPUT);

  pinMode(UZ_ZAD_VYSILAC, OUTPUT);
  pinMode(UZ_ZAD_PRIJIMAC, INPUT);

  pinMode(MOTOR_PA, OUTPUT);
  pinMode(MOTOR_PB, OUTPUT);
  pinMode(MOTOR_LA, OUTPUT);
  pinMode(MOTOR_LB, OUTPUT);

  digitalWrite(MOTOR_PA, LOW);
  digitalWrite(MOTOR_PB, LOW);
  digitalWrite(MOTOR_LA, LOW);
  digitalWrite(MOTOR_LB, LOW);

  pinMode(BLINKR_PRAVY, OUTPUT);
  pinMode(BLINKR_LEVY, OUTPUT);
  pinMode(ZPATECKA, OUTPUT);

  pinMode(LASER, INPUT);

  delay(1000);  // 1s prodleva pro dobeh periferii
}

////////////////////////////////////////////////////////////////////////////////////////////////////////
// Smycka hlavniho programu //////////////////////////////////////////////

void loop() {
  mereni_vzdalenosti_predni();  //Zmer predni vzdalenost
  mereni_vzdalenosti_zadni();   //Zmer zadni vzdalenost
  mereni_laser();               //Zmer spodni vzdalenost
  set_mod_a();                  //Nastav MOD A (je na zemi a muze jezdit ci ne)
  set_mod_b();                  //Nastav MOD B (dle prekazek)
  motory_mod();                 //Nastav MOD M (rychlost motoru dle rezimu v MODECH A a B, popojizdeni)
  set_svetla();                 //Nastav svetla
  zobrazeni_mereni();           //Zobraz hodnoty a mody na displeji
  //tone(PIEZO, 100, 500);    //Piezo frekvence Hz a trvani, test
}

////////////////////////////////////////////////////////////////////////////////////////////////////////
// Podprogram zobrazeni na displeji //////////////////////////////////////////////

void zobrazeni_mereni() {

  display.clearDisplay();               //Vymaze displej s predeslymi udaji
  display.setTextSize(1);               //Velikost bodu textu na 1px
  display.setTextColor(SSD1306_WHITE);  //Barva textu bila

  display.setCursor(0, 0);  //Pozice kurzoru
  display.println((String) "    ROBOAUTO STAV");

  display.setCursor(10, 14);  //Pozice kurzoru
  display.println((String) "MOD A: " + MOD_A);

  display.setCursor(70, 14);  //Pozice kurzoru
  display.println((String) "MOD B: " + MOD_B);

  display.setCursor(0, 28);  //Pozice kurzoru
  display.println((String) "IL D: " + M_LASER_DOLNI);
  display.setCursor(65, 28);  //Pozice kurzoru
  display.println((String) "us");

  display.setCursor(0, 40);  //Pozice kurzoru
  display.println((String) "UZ P: " + M_UZ_PREDNI);
  display.setCursor(65, 40);  //Pozice kurzoru
  display.println((String) "cm");

  display.setCursor(0, 52);  //Pozice kurzoru
  display.println((String) "UZ Z: " + M_UZ_ZADNI);
  display.setCursor(65, 52);  //Pozice kurzoru
  display.println((String) "cm");

  //display.drawCircle(110, 40, 12,SSD1306_WHITE);   //Prazdny kruh pro test
  //display.fillCircle(110, 40, 12,SSD1306_WHITE);   //Plny kruh pro test
  //display.drawTriangle(104, 34, 116, 34, 110, 46,SSD1306_WHITE);   //Prazdny trojuhelnik dolu pro test
  display.drawTriangle(104, 46, 116, 46, 110, 34, SSD1306_WHITE);  //Prazdny trojuhlenik nahoru

  display.display();  //Vypis na displej
}

////////////////////////////////////////////////////////////////////////////////////////////////////////
// Podprogram mereni vzdalenosti predni //////////////////////////////////////////////

void mereni_vzdalenosti_predni() {
  digitalWrite(UZ_PRED_VYSILAC, LOW);  //Na 2us GND pro vypnuti UZ
  delayMicroseconds(2);
  digitalWrite(UZ_PRED_VYSILAC, HIGH);  //Na 5us 5V pro zapnuti UZ
  delayMicroseconds(5);
  digitalWrite(UZ_PRED_VYSILAC, LOW);                //Vypnuti UZ
  UZ_PRED_ODEZVA = pulseIn(UZ_PRED_PRIJIMAC, HIGH);  //Trvani do zachyceni odrazu UZ
  UZ_PRED_VZDALENOST = UZ_PRED_ODEZVA / 58.31;       //Prepocet podle rychlosti zvuku pri 20°C
  M_UZ_PREDNI = UZ_PRED_VZDALENOST;
}

////////////////////////////////////////////////////////////////////////////////////////////////////////
// Podprogram mereni vzdalenosti zadni //////////////////////////////////////////////

void mereni_vzdalenosti_zadni() {
  digitalWrite(UZ_ZAD_VYSILAC, LOW);  //Na 2us GND pro vypnuti UZ
  delayMicroseconds(2);
  digitalWrite(UZ_ZAD_VYSILAC, HIGH);  //Na 5us 5V pro zapnuti UZ
  delayMicroseconds(5);
  digitalWrite(UZ_ZAD_VYSILAC, LOW);               //Vypnuti UZ
  UZ_ZAD_ODEZVA = pulseIn(UZ_ZAD_PRIJIMAC, HIGH);  //Trvani do zachyceni odrazu UZ
  UZ_ZAD_VZDALENOST = UZ_ZAD_ODEZVA / 58.31;       //Prepocet podle rychlosti zvuku pri 20°C
  M_UZ_ZADNI = UZ_ZAD_VZDALENOST;
}

////////////////////////////////////////////////////////////////////////////////////////////////////////
// Podprogram mereni vzdalenosti spodni //////////////////////////////////////////////

void mereni_laser() {
  uint16_t distance = 0;
  M_LASER_DOLNI = pulseIn(LASER, HIGH);
  //distance=LASER_TRVANI/10;  // prepocet na mm
  //M_LASER_DOLNI = distance;  // prepocet na mm
}

////////////////////////////////////////////////////////////////////////////////////////////////////////
// Podprogram pro konfiguraci rychlosti motoru //////////////////////////////////////////////

void motory_mod_RUN() {
  //Stop
  if (MOD_M == 0) {
    MOTOR_P_VPRED = 0;  //Motor pravy vpred - Hodnota PWM pro rychlost 0-255
    MOTOR_P_ZPET = 0;   //Motor pravy zpet - Hodnota PWM pro rychlost 0-255
    MOTOR_L_VPRED = 0;  //Motor levy vpred - Hodnota PWM pro rychlost 0-255
    MOTOR_L_ZPET = 0;   //Motor levy zpet - Hodnota PWM pro rychlost 0-255
    pohon_motor();      //Podprogram pro nastaveni rychlosti
  }

  //Vpred
  if (MOD_M == 1) {
    MOTOR_P_VPRED = 140;
    MOTOR_P_ZPET = 0;
    MOTOR_L_VPRED = 140;
    MOTOR_L_ZPET = 0;
    pohon_motor();
  }

  //Vzad
  if (MOD_M == 2) {
    MOTOR_P_VPRED = 0;
    MOTOR_P_ZPET = 140;
    MOTOR_L_VPRED = 0;
    MOTOR_L_ZPET = 140;
    pohon_motor();
  }

  //Doleva
  if (MOD_M == 3) {
    MOTOR_P_VPRED = 230;
    MOTOR_P_ZPET = 0;
    MOTOR_L_VPRED = 0;
    MOTOR_L_ZPET = 230;
    pohon_motor();
  }

  //Doprava
  if (MOD_M == 4) {
    MOTOR_P_VPRED = 0;
    MOTOR_P_ZPET = 230;
    MOTOR_L_VPRED = 230;
    MOTOR_L_ZPET = 0;
    pohon_motor();
  }
}

////////////////////////////////////////////////////////////////////////////////////////////////////////
// Podprogram pro ovladani motoru, nastaveni rychlosti //////////////////////////////////////////////

void pohon_motor() {
  analogWrite(MOTOR_PA, MOTOR_P_VPRED);
  analogWrite(MOTOR_PB, MOTOR_P_ZPET);
  analogWrite(MOTOR_LA, MOTOR_L_VPRED);
  analogWrite(MOTOR_LB, MOTOR_L_ZPET);
}

////////////////////////////////////////////////////////////////////////////////////////////////////////
// Podprogram pro zpomaleni pohybu prerusovanou jizdou //////////////////////////////////////////////

void motory_mod() {
  //Pohon vychozi
  if ((MOD_M_CAS_RUN == 0) and (MOD_M_CAS_STOP == 0)) {
    MOD_M_CAS_RUN = millis();
    motory_mod_RUN();
  }

  //Pohon stop
  if ((MOD_M_CAS_RUN != 0) and (MOD_M_CAS_STOP == 0) and (MOD_M_CAS_RUN + 50 < millis())) {
    MOD_M_CAS_STOP = millis();
    MOTOR_P_VPRED = 0;
    MOTOR_P_ZPET = 0;
    MOTOR_L_VPRED = 0;
    MOTOR_L_ZPET = 0;
    pohon_motor();
    MOD_M_CAS_RUN = 0;
  }

  //Pohon aktivni
  if ((MOD_M_CAS_RUN == 0) and (MOD_M_CAS_STOP != 0) and (MOD_M_CAS_STOP + 50 < millis())) {
    MOD_M_CAS_RUN = millis();
    motory_mod_RUN();
    MOD_M_CAS_STOP = 0;
  }
}

////////////////////////////////////////////////////////////////////////////////////////////////////////
// Podprogram pro zmenu modu zvednutim auta //////////////////////////////////////////////

void set_mod_a() {

  //Po zapnuti napajeni stoji
  //Po zvednuti stoji
  //Po polozeni jede
  //Po daslim zvednuti stoji a po polozeni take (parkovani)

  if ((MOD_A == 0) and (M_LASER_DOLNI > 1000)) {
    MOD_A = 1;
  }

  if ((MOD_A == 1) and (M_LASER_DOLNI < 700)) {
    MOD_A = 2;
  }

  if ((MOD_A == 2) and (M_LASER_DOLNI > 1000)) {
    MOD_A = 3;
  }

  if ((MOD_A == 3) and (M_LASER_DOLNI < 700)) {
    MOD_A = 0;
  }
}

////////////////////////////////////////////////////////////////////////////////////////////////////////
// Podprogram pro zmenu modu prekazkami //////////////////////////////////////////////
// Pouzity UZ ukazuje hodnotu vetsi nez 2000cm i kdyz je prekazka tesne u u nej, proto v podmince
// Zpozdeni pomoci millis je pouzito pro zastaveni motoru pred zmenou smeru jizdy

void set_mod_b() {

  // Nesmi se pohybovat kdyz je s autem rucne zvedano dle MOD A
  if (MOD_A != 2) {
    MOD_M = 0;
    motory_mod();
  }

  //Pokud je na zemi dle MOD A
  if (MOD_A == 2) {

    //Nahodna zmena smeru jizdy
    if (MOD_B == 1) {
      int rndNumberSmer = random(0, 1000);
      if (rndNumberSmer > 990) {
        digitalWrite(ZPATECKA, HIGH);
        MOD_M = 0;
        motory_mod();
        MOTOR_CAS = millis();
        MOD_B = 40;
      }
    }

    //Stop kdyz je tesna prekazka vpredu a vzadu
    if ((MOD_B == 0) or (MOD_B == 1) or (MOD_B == 9)) {
      if ((M_UZ_PREDNI < 5) and (M_UZ_ZADNI < 5)) {
        MOD_M = 0;
        motory_mod();
        MOD_B = 9;
        digitalWrite(BLINKR_PRAVY, LOW);
        digitalWrite(BLINKR_LEVY, LOW);
        digitalWrite(ZPATECKA, LOW);
      }
    }

    //Dopredu
    if ((MOD_B == 0) or (MOD_B == 1) or (MOD_B == 9)) {
      if ((M_UZ_PREDNI >= 25) and (M_UZ_PREDNI < 2000)) {
        MOD_M = 1;
        motory_mod();
        MOD_B = 1;
        digitalWrite(BLINKR_PRAVY, LOW);
        digitalWrite(BLINKR_LEVY, LOW);
        digitalWrite(ZPATECKA, LOW);
      }
    }

    //Stop kdyz je prekazka vpredu
    if ((MOD_B == 0) or (MOD_B == 1)) {
      if ((M_UZ_PREDNI < 25) or (M_UZ_PREDNI > 2000)) {
        MOTOR_CAS = millis();
        MOD_M = 0;
        motory_mod();
        MOD_B = 2;
        digitalWrite(ZPATECKA, HIGH);
      }
    }

    //Couvani po prekazce vpredu
    if ((MOD_B == 2) and (POCET_COUVANI < 2) and (MOTOR_CAS + 300 < millis())) {
      if (((M_UZ_PREDNI < 20) or (M_UZ_PREDNI > 2000)) and (M_UZ_ZADNI >= 20)) {
        MOD_M = 2;
        motory_mod();
        MOD_B = 3;
        digitalWrite(ZPATECKA, HIGH);
        POCET_COUVANI = POCET_COUVANI + 1;
      }
    }

    //Couvani po prekazce vpredu
    if ((MOD_B == 2) and (POCET_COUVANI >= 2) and (MOTOR_CAS + 300 < millis())) {
      if (((M_UZ_PREDNI < 20) or (M_UZ_PREDNI > 2000)) and (M_UZ_ZADNI >= 25)) {
        MOD_M = 2;
        motory_mod();
        MOD_B = 4;
        digitalWrite(ZPATECKA, HIGH);
        MOTOR_CAS = millis();
        POCET_COUVANI = 0;
      }
    }

    //Couvani po prekazce vpredu ALT
    if ((MOD_B == 2) and (MOTOR_CAS + 300 < millis())) {
      if ((M_UZ_PREDNI > 20) and (M_UZ_PREDNI < 2000) or (M_UZ_ZADNI < 25)) {
        MOTOR_CAS = millis();
        MOD_M = 2;
        motory_mod();
        MOD_B = 4;
        digitalWrite(ZPATECKA, HIGH);
        POCET_COUVANI = 0;
      }
    }

    //Stop po couvani po prekazce vpredu
    if (MOD_B == 3) {
      if ((M_UZ_PREDNI >= 20) and (M_UZ_PREDNI < 2000) and (M_UZ_ZADNI > 20)) {
        MOTOR_CAS = millis();
        MOD_M = 0;
        motory_mod();
        MOD_B = 4;
        digitalWrite(ZPATECKA, HIGH);
      }
    }

    //Volba smeru otaceni nahodne
    if ((MOD_B == 4) and (MOTOR_CAS + 300 < millis())) {
      int rndNumber = random(0, 10);
      if (rndNumber <= 5) { MOD_B = 5; }
      if (rndNumber > 5) { MOD_B = 6; }
      digitalWrite(ZPATECKA, LOW);
      POCET_COUVANI = 0;
    }

    //Otaceni doleva smykem doleva
    if (MOD_B == 5) {
      if (((M_UZ_PREDNI < 50) or (M_UZ_PREDNI > 2000)) and (M_UZ_ZADNI >= 15)) {
        MOD_M = 3;
        motory_mod();
        MOD_B = 5;
        digitalWrite(BLINKR_PRAVY, LOW);
        digitalWrite(BLINKR_LEVY, HIGH);
      }
    }

    //Otaceni doprava smykem doprava
    if (MOD_B == 6) {
      if (((M_UZ_PREDNI < 50) or (M_UZ_PREDNI > 2000)) and (M_UZ_ZADNI >= 15)) {
        MOD_M = 4;
        motory_mod();
        MOD_B = 6;
        digitalWrite(BLINKR_PRAVY, HIGH);
        digitalWrite(BLINKR_LEVY, LOW);
      }
    }

    //Stop po otaceni smykem
    if ((MOD_B == 5) or (MOD_B == 6)) {
      if ((M_UZ_PREDNI >= 50) and (M_UZ_PREDNI < 2000) and (M_UZ_ZADNI >= 15)) {
        MOD_M = 0;
        motory_mod();
        MOD_B = 7;
        MOTOR_CAS = millis();
        digitalWrite(BLINKR_PRAVY, LOW);
        digitalWrite(BLINKR_LEVY, LOW);
      }
    }

    //Stop po prekazce vepredu i vzadu
    if ((MOD_B == 0) or (MOD_B == 2) or (MOD_B == 3) or (MOD_B == 1) or (MOD_B == 9)) {
      if ((M_UZ_PREDNI < 14) and (M_UZ_ZADNI < 14)) {
        MOTOR_CAS = millis();
        MOD_M = 0;
        motory_mod();
        MOD_B = 4;
        digitalWrite(BLINKR_PRAVY, LOW);
        digitalWrite(BLINKR_LEVY, LOW);
        digitalWrite(ZPATECKA, LOW);
      }
    }

    //Prepnuti na vychozi stav
    if ((MOD_B == 7) and (MOTOR_CAS + 300 < millis())) {
      MOD_B = 0;
    }

    //Nahodna zmena smeru pro jizde vpred otacenim
    if ((MOD_B == 40) and (MOTOR_CAS + 300 < millis())) {
      int rndNumber = random(0, 10);
      if (rndNumber <= 5) { MOD_B = 50; }
      if (rndNumber > 5) { MOD_B = 60; }
      digitalWrite(ZPATECKA, LOW);
      MOTOR_CAS = millis();
    }

    //Otaceni doleva smykem - nahodne
    if (MOD_B == 50) {
      if (MOTOR_CAS + 500 > millis()) {
        MOD_M = 3;
        motory_mod();
        MOD_B = 50;
        digitalWrite(BLINKR_PRAVY, LOW);
        digitalWrite(BLINKR_LEVY, HIGH);
      }
    }

    //Otaceni doprava smykem - nahodne
    if (MOD_B == 60) {
      if (MOTOR_CAS + 500 > millis()) {
        MOD_M = 4;
        motory_mod();
        MOD_B = 60;
        digitalWrite(BLINKR_PRAVY, HIGH);
        digitalWrite(BLINKR_LEVY, LOW);
      }
    }

    //Konec otaceni smykem - nahodne
    if ((MOD_B == 60) or (MOD_B == 50)) {
      if (MOTOR_CAS + 500 < millis()) {
        MOD_M = 0;
        motory_mod();
        MOD_B = 7;
        MOTOR_CAS = millis();
        digitalWrite(BLINKR_PRAVY, LOW);
        digitalWrite(BLINKR_LEVY, LOW);
      }
    }
  }
}


////////////////////////////////////////////////////////////////////////////////////////////////////////
// Podprogram pro ovladani svetel //////////////////////////////////////////////

void set_svetla() {

  if ((MOD_A == 1) or (MOD_A == 3)) {
    digitalWrite(ZPATECKA, LOW);
    if ((BLINKR == 0) and (BLINKR_CAS + 100 < millis())) {
      BLINKR_CAS = millis();
      BLINKR = 1;
      digitalWrite(BLINKR_PRAVY, HIGH);
      digitalWrite(BLINKR_LEVY, HIGH);
    }
    if ((BLINKR == 1) and (BLINKR_CAS + 100 < millis())) {
      BLINKR_CAS = millis();
      BLINKR = 0;
      digitalWrite(BLINKR_PRAVY, LOW);
      digitalWrite(BLINKR_LEVY, LOW);
    }
  }

  if (MOD_A == 0) {
    digitalWrite(BLINKR_PRAVY, HIGH);
    digitalWrite(BLINKR_LEVY, HIGH);
    digitalWrite(ZPATECKA, LOW);
  }
}

////////////////////////////////////////////////////////////////////////////////////////////////////////