forum.hwkitchen.cz

Kód: Vybrat vše

void gyroskop() {
  accelgyro.getAcceleration(&ax, &ay, &az);       // zjistí všechny hodnoty z akcelerometru
  
  ax_p = ax_p + ax;                               // sčítání potřebných hodnot 
  ay_p = ay_p + ay;
  az_p = az_p + az;

    x = ax_p;                      
    y = ay_p;
    z = az_p;
     
    // vypočteme sklon a náklon [°]
    angle_x = atan2(x, sqrt(square(y) + square(z)))/(pi/180);
    angle_y = atan2(y, sqrt(square(x) + square(z)))/(pi/180);
    angle_z = atan2(z, sqrt(square(x) + square(y)))/(pi/180);   
  
    ax_p = 0;
    ay_p = 0;
    az_p = 0;
}

Kód: Vybrat vše

void test_naklonu () {          // vyhodnocení náklonu z gyroskopu - při náklonu větším jak 20° v osách X nebo Y 

     gyroskop (); 
     if ((angle_x > 20 && angle_x < 235 ) or (angle_y > 20 && angle_y < 235 )){     // velký náklon = 1
       naklon = 1;
     }
     else {naklon = 0;}              // pohyb vodorovně - naklon = 0
}

Kód: Vybrat vše

// Hlavní posloupnost kroků ovládání pavoučka v závislosti na změřené vzdálenosti překážky ultrazvukem a
// na kontrole náklonu přes gyroskop (zastavit kráčení). 


Do tohoto programu potřebuji vložit podmínku která způsobí zastavení kráčení na základě vyhodnocení náklonu. Jakmile se pavouček nakloní - proveď toto. Např. zastavení, pípání, krok vzad apod. Vyzkoušel jsem snad všechny varianty a stejně to nefunguje. POMOC !!   a díky za jakékoliv návrhy.

  
void sonar ();
  
  static uint32_t timer = 0;    //počáteční hodnota pro časovač
  static uint8_t  step_ = 0;    //počáteční hodnota pro kroky    step_
           
  if (timer > millis())        //Vraťte se,pokud je nastavený čas větší než aktuální počet milisekund,pokud ne,
                               //pokračujte v následujících operacích.
    return;
    switch (step_)               //skákání na funkce "case" podle  step_
  {
//-----------------------------------------------------------------------------------------------------------------    
    case 0:                                

      zmerit_vzdalenost();                //změřit vzdálenost ultrazvukem vpředu a uložit do  vzdalenost - kmitání

      if (!Controller.isRunning()) {            //Pokračuj, dokud nebude ukončena akční skupina pohybu, knihovna Lobot

          if (vzdalenost > 160 ){ 
          Controller.runActionGroup(RYCHLE, 0);    // rychlé kráčení vpřed, 0- znamená neustále do přerušení
          step_ = 1;                                   
        }
      
        else if (vzdalenost >= 100 ){ 
          Controller.runActionGroup(POMALU, 0);    //kráčet rychle (nebo pomalu) vpřed    0- znamená neustále až do přerušení , RYCHLE
          step_ = 1;                                   //skočit na step 1
        }
        
        else if (vzdalenost >= 50 ) {
             Controller.runActionGroup(TEREN_POMALU, 0);    //kráčet  pomalu  vpřed    0- znamená neustále až do přerušení ,  "POMALU"
             step_ = 1;                                   //skočit na step 1
            }
            
        else if (vzdalenost >= 38 || vzdalenost == 0) {       
            Controller.runActionGroup(PLAZENI, 0);       //kráčet  nejpomaleji vpřed    0- znamená neustále až do přerušení
            step_ = 1;                                   //skočit na step 1
        } 
                
        else {                             
          Controller.stopActionGroup();   // zastavit kráčení
          step_ = 2;                      // skočit na  step 2
          timer = millis() + 500;         // zjišťuje stav každých 500 ms (původní tovární hodnota 500)
        }
      }
       break;                             // ukončení podmínky pro přepínání
      
//---------------------------------------------------------------------------------------------------------      
    case 1:                               // step 1
       test_naklonu ();                   //kontrola náklonu pomocí gyroskopu
       zmerit_vzdalenost ();              // podprogram ke změření vzdálenosti ultrazvukem vpředu
       
       if ((vzdalenost <= 38 && vzdalenost > 0) or (pocet_naklonu == 1)) {
                                  // Pokud je naměřená vzdálenost menší než specifikovaná vzdálenost pro vyhnutí se překážce (380 mm) 
                                  // nebo náklon z gyro pak zastavte všechny akční skupiny a přejděte ke kroku 2.
                                  
        Controller.stopActionGroup();       // zastavit
        sonarServo.stop ();                 // zastavení serva ultrazvuku
        step_ = 2;                          // skočit na step 2
        timer = millis() + 500;             // původní hodnota 500 
      }
      break;                                // ukončení kroku 1
      
//------------------------------------------------------------------------  kontrola VLEVO a VPRAVO  -----------------------------------      
    case 2:

      if (!Controller.isRunning()) {      // pokud stojí tak změř vzdálenosti vlevo a vpravo, jinak běž na step 3
        getAllDistance();                 // změřit vzdálenost ve všech třech směrech
        step_ = 3;                        // skočit na case 3
      } 
      else {
        timer = millis() + 500;           // původní hodnota 500 
      }
      break;                              // ukončení kroku 2
      
//----------------------------------------------------------------------------------------------------------------      
    case 3:
      static bool lastActionIsGoBack = false;        //Statická proměnná - zaznamenat, zda je poslední akce zpět
                                                
     if (((vzdalenost_vpred > 38) || (vzdalenost_vpred == 0)) && lastActionIsGoBack == false) {
        
        //Pokud je mezilehlá vzdálenost větší než zadaná vzdálenost pro vyhnutí se překážce (cca 380 mm) a poslední akcí není ustoupit,
        //vraťte se ke kroku 0. Posuzuje se, zda se poslední akce vrací zpět nebo ne, aby se zabránilo pádu programu do
        //smyčky zpět-"dopředu-"dozadu-"dopředu..."
        //      Když je poslední krok zpět, neprovede se vpřed

        step_ = 0;                     // návrat na krok 0 
        timer = millis() + 200;        // původní hodnota 200
        lastActionIsGoBack = false;
        break;
      }
      
      if ((((vzdalenost_vlevo >= vzdalenost_vpravo) && (vzdalenost_vlevo >= 38)) || vzdalenost_vlevo == 0) && vzdalenost_vpred > 20) {
        
        //Když je vzdálenost na levé straně měřená ultrazvukem větší než vzdálenost na pravé straně
        //větší než specifikovaná vzdálenost pro vyhnutí se překážce a vzdálenost naměřená uprostřed je větší než 200 mm
        //Účelem detekce vzdálenosti uprostřed je vyhnout se předmětům mezi dvěma předními nohami robota, které způsobí
        // že se robot nemůže otočit.
        
        if (!Controller.isRunning()) {                              //Počkejte, dokud nebude ukončena aktivní skupina, příkaz knihovny Lobot
          
          if (vzdalenost_vlevo > 80){                              // když je vzdálenost vlevo větší než 1000 tak boční přesun
            Controller.runActionGroup(PRESUN_DOLEVA_POMALU, 3);    // 3x přesunout doleva 
            goto presunL;                                 // přesunout se bokem doleva a pokračovat bez otáčení                                 
          }
          
          Controller.runActionGroup(DOLEVA, 4);           // 4x zatočit doleva nízký postoj, příkaz knihovny Lobot
          
          presunL:
          
          lastActionIsGoBack = false;               // Identifies that the last action is not back, kontrola kroku zpět
          step_ = 0;                                // zpět na krok 0 
          timer = millis() + 300;                   // původně 300
        }        
      }
      
      else if ((((vzdalenost_vpravo > vzdalenost_vlevo) && (vzdalenost_vpravo > 38)) || vzdalenost_vpravo == 0) && vzdalenost_vpred > 20) {
        // Když je minimální vzdálenost na pravé straně měřená ultrazvukem větší než minimální vzdálenost na levé straně
        // větší než specifikovaná vzdálenost pro vyhnutí se překážce a vzdálenost měřená uprostřed (mezi nohama)
        //je větší než 200 mm
        
        if (!Controller.isRunning()) {                //Počkat, dokud nebude ukončena akční skupina
          
          if (vzdalenost_vpravo > 80){                         // když je vzdálenost vpravo větší než 1000 tak boční přesun
            Controller.runActionGroup(PRESUN_DOPRAVA_POMALU, 3);
            
            goto presunR;                                 // přesunout se bokem doprava a pokračovat bez otáčení                                 
          }
          
          Controller.runActionGroup(DOPRAVA, 4);        // 4x zatočit na místě doprava
          
          presunR:
                    
          lastActionIsGoBack = false;         //Identifies that the last action is not back
          step_ = 0;                          // zpět na krok 0
          timer = millis() + 300;             // původně 300
        }           
      }
      
      else {
        Controller.runActionGroup(GO_BACK, 4);      // 4x kráčet zpět 
                
        lastActionIsGoBack = true;                   //poslední akce kráčení je zpět
        step_ = 2;                                   //skok  step 2
        timer = millis() + 300;                      // původně 300
      }
      break;
  }
}

void loop ()
sonar

Kód: Vybrat vše

boolean test_naklonu () {          // vyhodnocení náklonu z gyroskopu - při náklonu větším jak 20° v osách X nebo Y  // funkce bude vracet boolean

     gyroskop (); 
     if ((angle_x > 20 && angle_x < 235 ) or (angle_y > 20 && angle_y < 235 )){     // velký náklon = 1
       return true; // je to nakloněné
     } else {
       return false; // není to naklonene 
     }              // pohyb vodorovně - naklon = 0
}

Kód: Vybrat vše

    case 1:                               // step 1
       if(test_naklonu()){                   //kontrola náklonu pomocí gyroskopu
         //vráceno true, takže je to naklopený
         //proved prikazy pro zastavení pavouka
       } else {
       	 //vráceno false, takže je rovně
       	 //asi není třeba dělat nic, nebo pokračuj v chůzi
       }

Kód: Vybrat vše

void test_naklonu () {          // vyhodnocení náklonu z gyroskopu - při náklonu větším jak 20° v osách X nebo Y 
     gyroskop (); 
     if ((angle_x > 20 && angle_x < 235 ) or (angle_y > 20 && angle_y < 235 )){     // velký náklon = 1
       naklon = 1;
     }
     else {naklon = 0;}              // pohyb vodorovně - naklon = 0
}
// a nebo
void gyroskop() {
  accelgyro.getAcceleration(&ax, &ay, &az);       // zjistí všechny hodnoty z akcelerometru
  
  ax_p = ax_p + ax;                               // sčítání potřebných hodnot 
  ay_p = ay_p + ay;
  az_p = az_p + az;

    x = ax_p;                      
    y = ay_p;
    z = az_p;
     
    // vypočteme sklon a náklon [°]
    angle_x = atan2(x, sqrt(square(y) + square(z)))/(pi/180);
    angle_y = atan2(y, sqrt(square(x) + square(z)))/(pi/180);
    angle_z = atan2(z, sqrt(square(x) + square(y)))/(pi/180);   
  
    ax_p = 0;
    ay_p = 0;
    az_p = 0;
}

Kód: Vybrat vše

int a = 10; // toto je globální proměnná

int funkce1(){
int b = 20; // toto je proměnná pouze pro funkci1, vytvoří se vždy když je třeba funkci provést a potom se tato proměnná zničí a uvolní místo v paměti
return b; // pokud chceme zpřístupnit z venku pro čtení nějakou hodnotu, použijeme návratový příkaz
}

Serial.println(funkce1()); // vypíše výsledek funkce do linky
int vysledekFunkce = funkce1(); // uloží výsledek funkce do proměnné

int funkce2(){
  a = 20;  // toto je korektní příkaz, funkce přepíše globální proměnnou a (a tady hrozí riziko, že dvě různé funkce se o toto místo v paměti poperou)
  b = 30;  // tady ti kompilátor nahlásí chybu, že proměnná b není definována pro tento "scope" ... scope = to co je mezi { } u funkcí, cyklů, tříd atd
}

inf funkce3(){
int b = 10; // ano - tohle je správně, sice mám v programu 2x deklarovanou a definovanou proměnnou jménem b, ale tím, že každá náleží jiné funkce, tak se jejich jména neperou a kompilátor jasně rozezná jedno b od druhého b
}

Kód: Vybrat vše

void PIR () {

  if (!Controller.isRunning()) {                // Počkejte, dokud nebude ukončena akční skupina 
  Controller.runActionGroup(LEHNOUT, 1);      // lehnout
  delay(400);
  Controller.stopActionGroup();               // zastavit kroky
  sonarServo.write (90,230,true);             // zastavení serva
  digitalWrite (10,LOW);                      // zhasnutí LED blesku
  digitalWrite (11,LOW);                      // vynulování PIR
  delay (1000);
  
start1:
  if (( digitalRead(11) == HIGH) or (cekani >400 ) or (digitalRead(2) == HIGH)){     // čeká na PIR  a zvuk  nebo uplynutí určité doby
                                                                                     // v minutách ( x 60 )       
                                                                     
     pocitadlo = 0;                         // vynulování počtu kroků pavouka
     cekani = 0;                            // vynulování doby čekání v PIR - cca  ... min
  }
  
  else {
    OLED ();
    cekani = cekani + 1;                   // počítadlo čekání v PIR
    //delay (10);
    goto start1;  
  }
}
}   

Kód: Vybrat vše

void loop() {
    Controller.receiveHandle();                    // Funkce pro komunikaci serv
    cekani = cekani + 1;                           // počítadlo operací
    
       if(test_naklonu()){                   //kontrola náklonu pomocí gyroskopu
       PIR ();
       } 
       else {
       }    
       
    sonar();                              // hlavní smyčka vyhodnocování pohybu
    
//-----------------------------------       
  //svetlo = analogRead (A3);             // precte fotosenzor  -   A3
    if (digitalRead (3)== HIGH){           // digi fotosenzor - D3
        digitalWrite (10,HIGH); }       // zapnutí blesku při nedostatku světla -  pin D10
        else {
        digitalWrite (10,LOW);  }       // když je dostatek světla tak vypnout blesk
//----------------------------------------------------------------------------------------------------------
//    if (digitalRead (2)== HIGH){         // přečte zvukový senzor, při písknutí přejít na PIR
//    {
//------------------------------------

   if (pocitadlo >= 5) {PIR();}      //   počet kroků , zastavit a přejít na kontrolu PIR senzoru
 
   OLED ();                            // výpis na oled display
 }

//********************************************** KONEC LOOP *****************************************************************

Kód: Vybrat vše

/******************************************************
* servo ultrazvuk  - D12
* digi svetlo      - D3   
* zvuk             -    A     rozsah 0-1023    min cca 100   (stačí kolem 250-350)
* PIR              - D11 
* infra levý       - D      ?
* infra pravý      - D      ?
* ESP32 kamera     - D10
* RX               - D9  
* TX               - D8
* DISPLAY          - A0 , A1
* digi zvuk        - D2         INTERRUPT   (musí být jen D2 nebo D3) - skok na PIR  
* ultrazvuk Echo   - D6
* ultrazvuk Trig   - D7
* svetlo cidlo analog  -  A3
* 
* VOLNY            - 
* VOLNY            -           
* 
*****************************************************/

#include <Wire.h>
//#include "Servo.h"
#include "SoftwareSerial.h"
#include "LobotServoController.h"
#include <MPU6050.h>
#include <I2Cdev.h>
//#include <EEPROM.h>
#include <VarSpeedServo.h> 
#include "U8glib.h"

#define rxPin 9  //Serial communication interface between arduino and servo controller
#define txPin 8  

//--------------------------------------------------------------
//#define GO_FORWARD  26            // pomaleji vpřed, střední postoj (původní 26)
#define RYCHLE      30              // rychle vpřed, střední postoj
#define GO_BACK     27              // pomalu kráčet vzad, střední postoj 
#define DOLEVA      32              // rychle doleva, střední postoj
#define DOPRAVA     33              // rychle doprava, střední postoj
#define LEHNOUT     13              // čapnutí a čekat na PIR
#define NAHORU      25              // nahoru PIR
#define POMALU      26              // pomalu vpřed 
#define PRESUN_DOLEVA   7           // rychlý přesun vlevo
#define PRESUN_DOLEVA_POMALU  120   // pomalý přesun vlevo
#define PRESUN_DOPRAVA  8           // rychlý přesun vpravo
#define PRESUN_DOPRAVA_POMALU 121   // pomalý přesun vpravo
#define PLAZENI           100       // velmi pomalé šourání vpřed
#define POMALU_NAHORU     102       // pomalé zvednutí
#define TEREN_POMALU      110       // pomalá výsoká chůze v terenu
#define TEREN_POKUS       130       // velmi rychle vpřed, terén
#define POMALU_POKUS      131       // středně rychle vpřed, terén


//---------------------------------------------------------------

#define DALEKO                // vzdálenost větší než 800-1000  
#define BIAS  0               /*Odchylka serva. Upravte data podle skutečné situace, aby ultrazvuk směřoval přímo dopředu*/

#define I2C_ADDR    0x77


//*************************************Gyroskop ********************************************
const float pi = 3.141592;
int16_t ax, ay, az;
int16_t gx, gy, gz;
float x, y, z;
byte  angle_x, angle_y, angle_z, _angle_x, _angle_y, _angle_z;
long ax_p, ay_p, az_p;


//********************************      INTELIGENCE       ***********************************************************

int AI_pohyb;               // počet kroků pavouka
int AI_PIR;                 // počet zaregistrování pohybu
int AI_zvuk;                // počet zaregistrování zvuku
int AI_svetlo;              // počet zaregistrování světla
int AI_vyhodnoceni;         // vyhodnocení vstupů  

//*********************************************************************************************

int svetlo;
int pocitadlo = 0;
int cekani = 0;
int zvuk;
int pir1 = 0;
int naklon = 0;

  const int trigPin1 = 7;    // ultrazvuk
  const int echoPin1 = 6;    // ultrazvuk

int  vpred;
long vzdalenost1, vzdalenost2, vzdalenost3 ;
long odezva;

  int vzdalenost;
  int vzdalenost_vpred;          
  int vzdalenost_vlevo;    
  int vzdalenost_vpravo;   

VarSpeedServo sonarServo;
SoftwareSerial MySerial(rxPin, txPin);          //Software serial port instantiation
LobotServoController Controller(MySerial);      //Instantiated secondary development
MPU6050 accelgyro;
U8GLIB_SSD1306_128X64 oled(U8G_I2C_OPT_NONE);          // inicializace OLED displeje z knihovny U8glib
//------------------------------------------------------------------------------------------------------------------

bool WireWriteByte(uint8_t val)
{
    Wire.beginTransmission(I2C_ADDR);
    Wire.write(val);
    if( Wire.endTransmission() != 0 ) {
        return false;
    }
    return true;
}

bool WireWriteDataArray(  uint8_t reg,uint8_t *val,unsigned int len)
{
    unsigned int i;

    Wire.beginTransmission(I2C_ADDR);
    Wire.write(reg);
    for(i = 0; i < len; i++) {
        Wire.write(val[i]);
    }
    if( Wire.endTransmission() != 0 ) {
        return false;
    }
    return true;
}

bool WireReadDataByte(uint8_t reg, uint8_t &val)
{
    if (!WireWriteByte(reg)) {
        return false;
    }
    
    Wire.requestFrom(I2C_ADDR, 1);
    while (Wire.available()) {
        val = Wire.read();
    }
    return true;
}
int WireReadDataArray(   uint8_t reg, uint8_t *val, unsigned int len)
{
    unsigned char i = 0;
    
    /* Indicate which register we want to read from */
    if (!WireWriteByte(reg)) {
        return -1;
    }
    
    /* Read block data */
    Wire.requestFrom(I2C_ADDR, len);
    while (Wire.available()) {
        if (i >= len) {
            return -1;
        }
        val[i] = Wire.read();
        i++;
    }
    return i;
}

//********************************************************************************************* KMITANI SERVA  ***********

//************************************************************************************************************************
//******************************************** KMITANI SERVA  ************************************************************

void  zmerit_vzdalenost() {                   //čtení vzdálenosti ultrazvuku    "vpred = vzdálenost naměřená ultrazvukem"
      
    sonarServo.write (50,230,true);           // vzdálenost mírně vlevo 50°    230 - rychlost otáčení
    ultrazvuk ();
     vzdalenost = vpred;

    sonarServo.write (130,230,true);          // vzdálenost mírně vpravo 130°
    ultrazvuk ();
    if (vzdalenost > vpred) { vzdalenost = vpred; };

    sonarServo.write (90,230,true);            // vzdálenost přímo vpředu 90°
    ultrazvuk ();
    if (vzdalenost > vpred) { vzdalenost = vpred; };
}

//**************************************************************************************************************
void getAllDistance()            // změřit vzdálenost ve všech třech směrech
{
  sonarServo.write(180,220,true);            //servo sonaru DOLEVA
  delay (300);
  ultrazvuk();
  vzdalenost_vlevo = vpred;          //
  
  sonarServo.write(0,220,true);           //servo sonaru DOPRAVA
  delay (300);
  ultrazvuk();
  vzdalenost_vpravo = vpred;          //

  sonarServo.write(90,220,true);            // servo sonaru VPŘED
  delay (300);
  ultrazvuk();                          //
  vzdalenost_vpred = vpred;
}

//***************************************************************************************************************************************
//****************************************************************************************************************************************

void sonar(){
  
  static uint32_t timer = 0;    //počáteční hodnota pro časovač
  static uint8_t  step_ = 0;    //počáteční hodnota pro kroky    step_
           
  if (timer > millis())        //Vraťte se,pokud je nastavený čas větší než aktuální počet milisekund,pokud ne,
                               //pokračujte v následujících operacích.
    return;
    switch (step_)             //skákání na funkce "case" podle  step_
  {
//-----------------------------------------------------------------------------------------------------------------    
  case 0:                        // prázdné case pro další využití
        
//-----------------------------------------------------------------------------------------------------------------    
    case 1:                                 
    
      zmerit_vzdalenost();                //změřit vzdálenost vpředu a uložit do  "vzdalenost" - kmitání serva
 
        if (test_naklonu()) {                   //kontrola náklonu pomocí gyroskopu         vráceno true, takže je to naklopený
        PIR ();                                     //přejdi na PIR
       } 
           else {
                                           //vráceno false, takže je pavouk rovně
       }

        
      if (!Controller.isRunning()) {            //Počkejte, dokud nebude ukončena akční skupina pohybu
        
        if (vzdalenost > 160 ){ 
          Controller.runActionGroup(TEREN_POKUS, 0);    // původní RYCHLE,         POKUS je lepší
          pocitadlo = pocitadlo + 1;
          step_ = 2;                                   
        }
      
        else if (vzdalenost > 100 ){ 
          Controller.runActionGroup(POMALU_POKUS, 0);    //kráčet rychle (nebo pomalu) vpřed    0- znamená neustále až do přerušení , RYCHLE
          pocitadlo = pocitadlo + 1;
          step_ = 2;                                     //původní - skočit na step 1
        }
        
        else if (vzdalenost > 50 ) {
             Controller.runActionGroup(TEREN_POMALU, 0);    //kráčet  pomalu  vpřed    0- znamená neustále až do přerušení ,  "POMALU"
             pocitadlo = pocitadlo + 1;
             step_ = 2;                                     //původní - skočit na step 0
            }
            
        else if (vzdalenost > 38 || vzdalenost == 0) {       
            Controller.runActionGroup(PLAZENI, 0);       //kráčet  nejpomaleji vpřed    0- znamená neustále až do přerušení
            pocitadlo = pocitadlo + 1;
            step_ = 2;                                   //skočit na step 1
        } 
                
        else {                            // 
          Controller.stopActionGroup();   // zastavit kráčení
          step_ = 3;                      // skočit na  step 2 (3)
          timer = millis() + 500;         // zjišťuje stav každých 500 ms (původní hodnota 500)
        }
      }
       break;                             // ukončení podmínky 
      
//---------------------------------------------------------------------------------------------------------      
    case 2:                               // step 1
       zmerit_vzdalenost ();
        
      if (vzdalenost <= 38 && vzdalenost > 0 ) {
                                  // Pokud je naměřená vzdálenost menší než specifikovaná vzdálenost pro vyhnutí se překážce (380 mm)
                                  // zastavte všechny akční skupiny a přejděte ke kroku 2.
        Controller.stopActionGroup();       // zastavit
        sonarServo.stop ();
        step_ = 3;                          // skočit na step 2
        timer = millis() + 500;             // původní hodnota 500 
      }
      break;                                // ukončení podmínky kroku 1
      
//------------------------------------------------------------------------  kontrola VLEVO a VPRAVO  -----------------------------------      
    case 3:

      if (!Controller.isRunning()) {      // pokud stojí tak změř vzdálenosti vlevo a vpravo, jinak běž na step 3
        getAllDistance();                 // změřit vzdálenost ve všech třech směrech
        step_ = 4;                        // skočit na case 3 (4)- vyhodnocení vzdáleností po zastavení
      } 
      else {
        timer = millis() + 500;           // původní hodnota 500 
      }
      break;                              // ukončení kroku 2
      
//----------------------------------------------------------------------------------------------------------------      
    case 4:
      static bool lastActionIsGoBack = false;        //Statická proměnná - zaznamenat, zda je poslední akce zpět
                                                
     if (((vzdalenost_vpred > 38) || (vzdalenost_vpred == 0)) && lastActionIsGoBack == false) {
        
        //Pokud je mezilehlá vzdálenost větší než zadaná vzdálenost pro vyhnutí se překážce (cca 380 mm) a poslední akcí není ustoupit,
        //vraťte se ke kroku 0. Posuzuje se, zda se poslední akce vrací zpět nebo ne, aby se zabránilo pádu programu do
        //smyčky zpět-"dopředu-"dozadu-"dopředu..."
        //      Když je poslední krok zpět, neprovede se vpřed

        step_ = 0;                     // návrat na krok 0 
        timer = millis() + 200;        // původní hodnota 200
        lastActionIsGoBack = false;
        break;
      }
 //----------------------------------------------------------------------------------------------------------------------------------     
   if ((((vzdalenost_vlevo >= vzdalenost_vpravo) && (vzdalenost_vlevo >= 38)) || vzdalenost_vlevo == 0) && vzdalenost_vpred > 20) {
        
        //Když je vzdálenost na levé straně měřená ultrazvukem větší než vzdálenost na pravé straně
        //větší než specifikovaná vzdálenost pro vyhnutí se překážce a vzdálenost naměřená uprostřed je větší než 200 mm
        //Účelem detekce vzdálenosti uprostřed je vyhnout se předmětům mezi dvěma předními nohami robota, které způsobí
        // že se robot nemůže otočit.
        
       if (!Controller.isRunning()) {                     //Počkejte, dokud nebude ukončena aktivní skupina
          
          if (vzdalenost_vlevo >= 80){                             // když je vzdálenost vlevo větší než 1000 tak boční přesun
            Controller.runActionGroup(PRESUN_DOLEVA_POMALU, 3);    // 3x přesunout doleva 
            pocitadlo = pocitadlo + 1;
            step_ = 0;                                // zpět na krok 0 
            timer = millis() + 300;                   // původně 300
            //goto presunL;                                 // přesunout se bokem doleva a pokračovat bez otáčení                                 
          }
          
          if (vzdalenost_vlevo < 80) {
          Controller.runActionGroup(DOLEVA, 4);         // 4x zatočit doleva nízký postoj
          
          //presunL:
          pocitadlo = pocitadlo + 1;
          
          lastActionIsGoBack = false;               //Identifies that the last action is not back
          step_ = 0;                                // zpět na krok 0 
          timer = millis() + 300;                   // původně 300
          }  
        }
        break;
     } 
//----------------------------------------------------------------------------------------------------------------------------------------                 
   else if ((((vzdalenost_vpravo > vzdalenost_vlevo) && (vzdalenost_vpravo > 38)) || vzdalenost_vpravo == 0) && vzdalenost_vpred > 20) {
        // Když je minimální vzdálenost na pravé straně měřená ultrazvukem větší než minimální vzdálenost na levé straně
        // větší než specifikovaná vzdálenost pro vyhnutí se překážce a vzdálenost měřená uprostřed (mezi nohama)
        //je větší než 200 mm
        
        if (!Controller.isRunning()) {                //Počkat, dokud nebude ukončena akční skupina
          
          if (vzdalenost_vpravo >= 80){                         // když je vzdálenost vpravo větší než 1000 tak boční přesun
            Controller.runActionGroup(PRESUN_DOPRAVA_POMALU, 3);
              step_ = 0;                          // zpět na krok 0
              timer = millis() + 300;             // původně 300
            //goto presunR;                                 // přesunout se bokem doprava a pokračovat bez otáčení                                 
          }
          
          if (vzdalenost_vpravo < 80) {
          Controller.runActionGroup(DOPRAVA, 4);        // 4x zatočit doprava
          //presunR:
          pocitadlo = pocitadlo + 1;
                    
          lastActionIsGoBack = false;         //Identifies that the last action is not back
          step_ = 0;                          // zpět na krok 0
          timer = millis() + 300;             // původně 300
        }           
    }
    break; 
  }         
      else {
        Controller.runActionGroup(GO_BACK, 4);      // 4x kráčet zpět 
        
          pocitadlo = pocitadlo + 1;
                
        lastActionIsGoBack = true;                   //poslední akce kráčení je zpět
        step_ = 3;                                   //skok  step 2
        timer = millis() + 300;                      // původně 300
      break;
      }
  }     
} 
  
//************************************************************************************************************************
//************************************************************************************************************************

void setup() 
{
  Wire.begin();
  Serial.begin(9600);                                //Initialize serial port 0, baud rate 9600
  MySerial.begin(9600);                              //Initialize serial port , baud rate 9600
  sonarServo.attach(12);                             // pin 12
  sonarServo.write(90);                              // počáteční hodnota
  accelgyro.initialize();

  //attachInterrupt(0, PIR, RISING);    //při změně na pinu 2 z LOW na HIGH vyvolá podprogram "PIR"
  
  pinMode (10, OUTPUT);               // blesk osvětlení
  pinMode (11, INPUT);                 // PIR
  //pinMode (9, INPUT);          // digi výstup ze zvukového čidla
  pinMode (3, INPUT);           //  digi výstup fotosenzor
  pinMode (7, OUTPUT);           // trig ultrazvuku 1 predni  -  
  pinMode (6, INPUT);            // echo ultrazvuku 1         - 
  pinMode (A3, INPUT);           // analog výstup ze svetelného cidla
  //pinMode (, INPUT);          // analog výstup ze zvukového čidla
  //pinMode (13, INPUT);          // digi výstup ze zvuk čidla
   
//--------------------------------- STARTOVNI POZICE -------------------------------------------------------------------------- 
    
  Controller.runActionGroup(LEHNOUT, 1);      // původně OBRANA
  Controller.stopActionGroup();               // zastavit
  digitalWrite (10, HIGH);                    // zapnout blesk
  delay(1000);                                // mírné čekání
  digitalWrite (10, LOW);                     // vypnout blesk
  
  }

//*********************************************************************************************************************************************
//*********************************************************************************************************************************************

void loop() {
    Controller.receiveHandle();                    // Funkce pro komunikaci serv
    cekani = cekani + 1;                           // počítadlo operací
    
       if(test_naklonu()){                   //kontrola náklonu pomocí gyroskopu
       PIR ();
       } 
       else {
       }    
       
    sonar();                              // hlavní smyčka vyhodnocování pohybu
    
//-----------------------------------       
  //svetlo = analogRead (A3);             // precte fotosenzor  -   A3
    if (digitalRead (3)== HIGH){           // digi fotosenzor - D3
        digitalWrite (10,HIGH); }       // zapnutí blesku při nedostatku světla -  pin D10
        else {
        digitalWrite (10,LOW);  }       // když je dostatek světla tak vypnout blesk
//----------------------------------------------------------------------------------------------------------
//    if (digitalRead (2)== HIGH){         // přečte zvukový senzor, při písknutí přejít na PIR
//    {
//------------------------------------

   if (pocitadlo >= 5) {PIR();}      //   počet kroků , zastavit a přejít na kontrolu PIR senzoru
 
   OLED ();                            // výpis na oled display
 }

//********************************************** KONEC LOOP *****************************************************************
//***************************************************************************************************************************
//***************************************************************************************************************************


void gyroskop() {
  accelgyro.getAcceleration(&ax, &ay, &az);       // zjistí všechny hodnoty z akcelerometru
  
  ax_p = ax_p + ax;                               // sčítání potřebných hodnot 
  ay_p = ay_p + ay;
  az_p = az_p + az;

    x = ax_p;                      
    y = ay_p;
    z = az_p;
     
    // vypočteme sklon a náklon [°]
    angle_x = atan2(x, sqrt(square(y) + square(z)))/(pi/180);
    angle_y = atan2(y, sqrt(square(x) + square(z)))/(pi/180);
    angle_z = atan2(z, sqrt(square(x) + square(y)))/(pi/180);   
  
    ax_p = 0;
    ay_p = 0;
    az_p = 0;
}

//************************************          KONTROLA náklonu   *********************************************************************
//**************************************************************************************************************************************

boolean test_naklonu () {          // vyhodnocení náklonu z gyroskopu - při náklonu větším jak 20° v osách X nebo Y  // funkce bude vracet boolean

     gyroskop (); 
     if ((angle_x > 20 && angle_x < 235 ) or (angle_y > 20 && angle_y < 235 )){     // velký náklon = 1
       return true;                   // je to nakloněné
     } else {
       return false;                  // není to naklonene 
     }                                // pohyb vodorovně - naklon = 0
}
 

//***************************************************************************************************************************************
//***************************************************************************************************************************************

void OLED (){
   gyroskop ();
   svetlo = analogRead (A3);       // svetlo
   pir1   = digitalRead (11);      // PIR         // hodnota stavu PIR pro oled displej
   ultrazvuk ();
   vzdalenost = vpred;             // zjištěná vzdálenost přímo vpředu
  
   oled.firstPage();  
  do
   {
   oled.drawLine(0,1,128,1);      // rovná čára, levý horní roh - pravý horní roh

   oled.setFont(u8g_font_7x14);   // nastavení fontu
                                  
   oled.setPrintPos(2,14);        // nastavení pozice kurzoru
   oled.print("vzd.:");           
   oled.setPrintPos(37,14);       // nastavení pozice kurzoru
   oled.print(vzdalenost);             // -------------------         výpis vzdálenosti sonar

   oled.setPrintPos(64,14);        // nastavení pozice kurzoru
   oled.print("krok:");           // výpis informace
   oled.setPrintPos(100,14);       // nastavení pozice kurzoru
   oled.print(pocitadlo);             // --------------------        výpis počtu kroků

   oled.setPrintPos(2,29);        // nastavení pozice kurzoru
   oled.print("zvuk:");           // výpis informace
   oled.setPrintPos(37,29);       // nastavení pozice kurzoru
   oled.print(zvuk);             // --------------------        výpis hlasitosti zvuku

   oled.setPrintPos(64,29);       // nastavení pozice kurzoru
   oled.print("foto:");            // výpis informace
   oled.setPrintPos(100,29);       // nastavení pozice kurzoru
   oled.print(naklon);             // ---------------------        výpis fotočidla  (stav náklonu gyro)

   oled.setPrintPos(2,44);        // nastavení pozice kurzoru
   oled.print("poc:      ");      // výpis informace 
   oled.setPrintPos(37,44);       // nastavení pozice kurzoru
   oled.print(cekani);             // ---------------------- cekani

   oled.setPrintPos(85,44);        // nastavení pozice kurzoru
   oled.print("P:  ");             // výpis informace
   oled.setPrintPos(100,44);       // nastavení pozice kurzoru
   oled.print(pir1);               // ----------------------        výpis PIR


  oled.setPrintPos(2,59);        // nastavení pozice kurzoru
   oled.print("XYZ:");           // výpis informace
   oled.setPrintPos(31,59);       // nastavení pozice kurzoru
   oled.print(angle_x);             // ----------------------        výpis náklonu   Gyroskop
   oled.setPrintPos(59,59);       // nastavení pozice kurzoru
   oled.print(angle_y);             // ----------------------        výpis náklonu   Gyroskop
   oled.setPrintPos(84,59);       // nastavení pozice kurzoru
   oled.print(angle_z);             // ----------------------        výpis náklonu   Gyroskop

  //oled.drawLine(0,61,128,61);     // rovná čára (od souřadnic -> do souřadnic)
    } 
   while(oled.nextPage());

   delay(10);                    // "obnovovací frekvence displeje"
   }

//****************************************************************************************************************************************
   void PIR () {

  if (!Controller.isRunning()) {                // Počkejte, dokud nebude ukončena akční skupina 
  Controller.runActionGroup(LEHNOUT, 1);      // lehnout
  delay(400);
  Controller.stopActionGroup();               // zastavit kroky
  sonarServo.write (90,230,true);             // zastavení serva
  digitalWrite (10,LOW);                      // zhasnutí LED blesku
  digitalWrite (11,LOW);                      // vynulování PIR
  delay (1000);
  
start1:
  if (( digitalRead(11) == HIGH) or (cekani >400 ) or (digitalRead(2) == HIGH)){     // čeká na PIR  a zvuk  nebo uplynutí určité doby
                                                                                     // v minutách ( x 60 )       
                                                                     
     pocitadlo = 0;                         // vynulování počtu kroků pavouka
     cekani = 0;                            // vynulování doby čekání v PIR - cca  ... min
  }
  
  else {
    OLED ();
    cekani = cekani + 1;                   // počítadlo čekání v PIR
    //delay (10);
    goto start1;  
  }
}
}   

//----------------------------------------------------------------------------------------------------------------------------------

void ultrazvuk() {
  sonarServo.write (90,200,true);

  digitalWrite(7, LOW);
  delayMicroseconds(2);
  digitalWrite(7, HIGH);
  delayMicroseconds(5);
  digitalWrite(7, LOW);
  odezva = pulseIn(6, HIGH);
  vpred = odezva / 58.31;        // vzdálenost v cm
  delay (10);
  }
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

void kontrola_zvuku (){
  if (digitalRead (11) == HIGH) {        // po zvuku nebo poklepání přejít na PIR
   PIR();                               // zastavit a přejít na kontrolu PIR senzoru
 }
}
//**********************************************************************************************************************