Ukončuje příkaz ifMám k tomu otázku. Jaký význam má ten středník za složenou závorkou ?
Pavouk Hiwonder
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.
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.
Re: Pavouk Hiwonder
Re: Pavouk Hiwonder
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ě
. 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ě 
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ě
. 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ě Re: Pavouk Hiwonder
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 ° :
Program ke kontrole náklonu. Pokud je náklon v ose X nebo Y větší jak 20° tak ZASTAV kráčení:
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;
}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 ()
sonarRe: Pavouk Hiwonder
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í?
Re: Pavouk Hiwonder
No prvně si musíš přepsat funkci na testování náklonu tak, aby uměla vůbec vracet výsledek ... true/false stačí ...
pak stím budeš pracovat takto
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
}
Re: Pavouk Hiwonder
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
}
Re: Pavouk Hiwonder
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é:
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:
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;
}
}
} 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 *****************************************************************Re: Pavouk Hiwonder
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
Re: Pavouk Hiwonder
Čí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"
Re: Pavouk Hiwonder
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á)
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
}
}
//**********************************************************************************************************************
Kdo je online
Uživatelé prohlížející si toto fórum: Žádní registrovaní uživatelé a 2 hosti
