Pavouk Hiwonder

Nedaří se vám s projektem a nenašli jste vhodné místo, kde se zeptat? Napište sem.
Pravidla fóra
Tohle subfórum je určeno pro konzultaci ucelených nápadů, popřípadě řešení komplexnějších projektů, které opravdu není možné rozdělit na menší části.
Většinu problémů jde rozdělit na menší a ptát se na ně v konkrétních subfórech.
Odpovědět
Pablo74
Příspěvky: 58
Registrován: 03 lis 2019, 17:00

Re: Pavouk Hiwonder

Příspěvek od Pablo74 » 26 kvě 2024, 21:58

Mám k tomu otázku. Jaký význam má ten středník za složenou závorkou ?
Ukončuje příkaz if

Uživatelský avatar
gilhad
Příspěvky: 820
Registrován: 07 bře 2018, 11:22

Re: Pavouk Hiwonder

Příspěvek od gilhad » 27 kvě 2024, 02:21

Ano, mám tam pár znaků navíc, které kompilátor hravě zpracuje a zahodí a nebude z nich generovat žádný kód, ale mě to takhle přijde poněkud přehlednější a jasnější, co je odkud až kam a kde už je něco jiného.

Proto běžně u ifů používám složené závorky i pokud je tam jen jeden příkaz. Pak se mi tak snadno nestane, že bych chtěl tu podmíněnou část rozšírit, něco tam dopsal, ale i když by to třeba bylo na jednom řádku, tak by to překladač přeložil tak, že by se první příkaz provedl podmíněně a zbytek už nepodmíněně.

A proto nešetřím středníkama, středník navíc je sice interpretován jako prázdný příkaz, ale překladač to vyhodnotí správně a negeneruje pro něuj zbytečný kód a já líp vidím, jak jsem to zamýšlel.

Samozřejmě to jde napsat i bez těch zbytečných znaků a místo odřádkování použít jen mezeru, stejně jako odsazení nemusí být použité, celý program může být třeba jen pár extrémně dlouhých řádků a jména proměnných jen jednoznaková a klidně to můžu napsat i obfuskovaně :twisted: . Ale většinou se ke svému kódu vracím i později a to jsem radši, když si to napíšu čitelně a přehledně :lol:

luger
Příspěvky: 193
Registrován: 30 dub 2023, 11:06

Re: Pavouk Hiwonder

Příspěvek od luger » 29 kvě 2024, 11:45

Ještě se vracím ke kontrole náklonu pavoučka a vyhodnocení hodnot z gyroskopu. Vyhodnocování překročení zadaného náklonu funguje ok a přehledně ho kontroluji na oled displeji kde vidím všechny hodnoty. Knihovna MPU6050 asi pracuje na pozadí (nejsem si jistý).
Program ke zjištění hodnot náklonu ve ° :

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;
}
Program ke kontrole náklonu. Pokud je náklon v ose X nebo Y větší jak 20° tak ZASTAV kráčení:

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

luger
Příspěvky: 193
Registrován: 30 dub 2023, 11:06

Re: Pavouk Hiwonder

Příspěvek od luger » 29 kvě 2024, 12:54

jejda , někde s ztratila hlavní otázka : Kam mám do podprogramu "sonar" vrazit podmínku ke kontrole náklonu a aby se zastavilo kráčení?

Uživatelský avatar
kiRRow
Příspěvky: 1222
Registrován: 07 kvě 2019, 07:03
Bydliště: Opava

Re: Pavouk Hiwonder

Příspěvek od kiRRow » 29 kvě 2024, 15:05

No prvně si musíš přepsat funkci na testování náklonu tak, aby uměla vůbec vracet výsledek ... true/false stačí ...

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
}
pak stím budeš pracovat takto

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
       }

Uživatelský avatar
kiRRow
Příspěvky: 1222
Registrován: 07 kvě 2019, 07:03
Bydliště: Opava

Re: Pavouk Hiwonder

Příspěvek od kiRRow » 29 kvě 2024, 15:17

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;
}
Když na to koukám, tak vzhledem k absenci jakékoliv deklarace proměnných uvnitř funkcí počítám s tím, že máš všechno deklarováno globálně. Což je sice funkční řešení, ale z hlediska využití paměti neefektivní a z hlediska přístupu k paměti nebezpečné.

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
}

luger
Příspěvky: 193
Registrován: 30 dub 2023, 11:06

Re: Pavouk Hiwonder

Příspěvek od luger » 29 kvě 2024, 19:02

Diky moc kiRRow, ale bohužel nefunguje to. Použil jsem metodu s true/false a umístil podmínku do case 1, žádná reakce. Tak jsem ji dal i do loop a zase žádná reakce.

Pro upřenění - při náklonu by se měl pavouček schoulit a čekat (PIR) - je to trochu kostrbaté, ale neustále tam doplňuji nějaké funkce takže je to nepřehledné:

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;  
  }
}
}   
Jde o to že by při náklonu měl skočit do voidu PIR a to nedělá. U mé metody to občas fungovalo ale tak nějak nepředvídatelně.

LOOP je:

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 *****************************************************************

Uživatelský avatar
kiRRow
Příspěvky: 1222
Registrován: 07 kvě 2019, 07:03
Bydliště: Opava

Re: Pavouk Hiwonder

Příspěvek od kiRRow » 29 kvě 2024, 19:40

Protože ještě pořád sestavuješ ten program metodou pokus omyl. Na tom není nic špatného, časem to přejde, všichni jsme tak začli a kdo né, tak kecá.... Ta funkce by určitě fungovat měla - měla by vracet false, když bude pavouk v pohodě a true, když ho moc nakloníš. Problém bude spíše někde s umístěním volání té funkce, nebo s vyhodnocením a nebo s provedenými příkazy ... a proto je lepší poslat kód celý, až po té stačí posílat jen změny v určitých blocích

Uživatelský avatar
kiRRow
Příspěvky: 1222
Registrován: 07 kvě 2019, 07:03
Bydliště: Opava

Re: Pavouk Hiwonder

Příspěvek od kiRRow » 29 kvě 2024, 19:58

Čím konkrétněji se zeptáš a čím detailnější informace a materiál nám poskytneš, tím máš lepší šanci že ti někdo poradí. Jinak probíhá něco čemu zde říkáme "věštění z křišťálové koule" ... následován procesem co by se dal nazvat "tahaní z chlupatý deky"

luger
Příspěvky: 193
Registrován: 30 dub 2023, 11:06

Re: Pavouk Hiwonder

Příspěvek od luger » 29 kvě 2024, 20:34

Je zajímavé, že pokud je pocitadlo >= 5 tak se ta podmínka splní a skočí na PIR (v loop) a také v case 2 je podmínka k zjištění min. vzdálenosti a to zareaguje okamžitě a provede další příslušné akce, ale jakmile tam vložím i podmínku náklonu tak na ni nereaguje a jede si klidně dál. To mi hlava nebere. Určitě je to jen nějaká blbost, ale já ji tam prostě nevidím. Chce to zkušenější oko.

Posílám celý kód (snad se v tom někdo vyzná)

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
 }
}
//**********************************************************************************************************************



   

Odpovědět

Kdo je online

Uživatelé prohlížející si toto fórum: Žádní registrovaní uživatelé a 1 host