[arduino][czołg + ping] Prośba o sprawdzenie kodu

[Baritoshi]

Witam,

Bawię się dalej swoim czołgiem ( link do posta ) i postanowiłem założyć mu czujnik odległości US-015.

Musiałem napisać kod prawie że od nowa i jak to zwykle wtedy bywa, coś poszło nie tak.

Bot zamiast jechać do przodu i w razie natrafienia na coś wycofać się i obrócić w lewo, obraca się niemalże cały czas.

Czy mógłby ktoś być taki miły i zerknąć gdzie tu mam błąd? Prosiłbym o wyjaśnienie. Przyznaję, że jestem jeszcze programistycznym mugolem.

Dodam, że testowałem osobno czujnik odległości i działał (kod przepisałem do funkcji getDistance), robot też jest prawidłowo poskładany (wgrywam poprzednio działający soft i działa bez zarzutu)

oto kod:

//definicje pinów



//piny prędkości (PWM)niebieskie
int m1speed = 3; //pin IN2EN
int m2speed = 5; //pin IN1EN
//piny kierunku (GPIO)orange
int m1direction = 7; //pin IN1PH
int m2direction = 4; //pinIN2PH


//serwo i oczy - piny
int TrigPin = 11; //pin trig pogłączony NIEBIESKIM przewodem do pinu 11
int EchoPin = 10; //pin echo podłączony ŻÓŁTYM przewodem do pinu 10

boolean crash = false; //zmienna określająca czy robot jest zbyt blisko przeszkody

void setup ()
{
 pinMode (m1direction, OUTPUT);
 pinMode (m2direction, OUTPUT);
 pinMode (13, OUTPUT); //pin diody LED
 pinMode (12, OUTPUT); //pin buzzera
 pinMode (EchoPin, INPUT);
 pinMode (TrigPin, OUTPUT);



 //dźwięk powitalny
 tone (12, 600, 500);
 delay(500);
 tone (12, 1200, 500);
 delay(500);
 tone (12, 1800, 500);
 delay(500);

 //świecenie pinów
 for (int t = 1; t < 6; t++)
 {
   digitalWrite (13, HIGH);
   delay (500);
   digitalWrite (13, LOW);
   delay (500);
 }
}
void loop ()

//opis: komenda(czas, prędkość);
{

 //praca silników
 goForward (1000, 250);


}




//funkcja obliczająca odległość robota od przeszkody i zwracająca wartość w cm
int getDistance()
{
 int distance; // końcowa zmienna odległość w cm (zapisana jako integer)
 unsigned long Time_Echo_us = 0; //zmienna czasu echa
 unsigned long Len_mm = 0; // zmienna odległości zapisana jako long

 digitalWrite (TrigPin, HIGH); //wyślij sygnał do czujnika odległości
 delayMicroseconds(50); 
 digitalWrite(TrigPin, LOW);

 Time_Echo_us = pulseIn(EchoPin, HIGH); //nasłuchuj echa
 if((Time_Echo_us < 60000) && (Time_Echo_us > 1)) //jeśli dostaniesz odpowiedź
 {
   Len_mm = (Time_Echo_us*34/100)/2; //oblicz odległość
 }
 distance = Len_mm; //dodaj ją do zmiennej distance
 return distance; //wypluj wartość distance
}





void goForward (int duration, int pwm) //jazda do przodu - zmienna duration oznacza jak długo ma jechać, zmienna 2 z jaką prędkością
{
 long a,b;
 int dist = 0;
 boolean Move=true;
 a=millis();
 do{
   dist = getDistance();
   if (dist < 40)
   {
     crash=true;
   }
   if (crash==false)
   {
     digitalWrite (m1direction, HIGH); //ustawiam kierunek silników - załóżmy, że do przodu
     digitalWrite (m2direction, HIGH);
     analogWrite (m1speed, pwm); //ustawiam prędkość silników na zmienną pwm (0-255)
     analogWrite (m2speed, pwm);
   }
   if (crash==true)
   {
     tone (12, 1800, 500);
     goBackward(2000, 200);//tu trzeba dodać prędkość i czas
     rotateLeft(2000, 200);//tu trzeba dodać prędkość i czas
     crash = false;
   }
   b=millis()-a;
   if (b>=duration)
   {
     Move = false;
   }
 }
 while (Move!=false);
 analogWrite (m1speed, 0); //ustawiam prędkość silnikó na zero
 analogWrite (m2speed, 0);

}


void goBackward (int duration, int pwm) //jazda do tyłu - zmienna duration oznacza jak długo ma jechać, zmienna 2 z jaką prędkością
{
 digitalWrite (m1direction, LOW); //ustawiam kierunek silników - załóżmy, że do tyłu
 digitalWrite (m2direction, LOW);
 analogWrite (m1speed, pwm); //ustawiam prędkość silników na zmienną pwm (0-255)
 analogWrite (m2speed, pwm);
 delay (duration); //silniki pracują tak długo jak wskazuje zmienna duration
 analogWrite (m1speed, 0); //ustawiam prędkość silnikó na zero
 analogWrite (m2speed, 0);
}



//te zostawiamy dla świętego spokoju

void rotateLeft (int duration, int pwm) //obrót w lewo - zmienna duration oznacza jak długo ma jechać, zmienna 2 z jaką prędkością
{
 digitalWrite (m1direction, LOW); //ustawiam kierunek silników - jeden do przodu, drugi do tyłu
 digitalWrite (m2direction, HIGH);
 analogWrite (m1speed, pwm); //ustawiam prędkość silników na zmienną pwm (0-255)
 analogWrite (m2speed, pwm);
 delay (duration); //silniki pracują tak długo jak wskazuje zmienna duration
 analogWrite (m1speed, 0); //ustawiam prędkość silnikó na zero
 analogWrite (m2speed, 0);
}


void rotateRight (int duration, int pwm) //obrót w prawo - zmienna duration oznacza jak długo ma jechać, zmienna 2 z jaką prędkością
{
 digitalWrite (m1direction, HIGH); //ustawiam kierunek silników - jeden do tyłu, drugi do przodu
 digitalWrite (m2direction, LOW);
 analogWrite (m1speed, pwm); //ustawiam prędkość silników na zmienną pwm (0-255)
 analogWrite (m2speed, pwm);
 delay (duration); //silniki pracują tak długo jak wskazuje zmienna duration
 analogWrite (m1speed, 0); //ustawiam prędkość silnikó na zero
 analogWrite (m2speed, 0);
}

[Treker (Damian Szymański)]

Zmiennej distance nie przypisujesz żadnej wartości początkowej, przypisz jej dla pewności 0 przy deklaracji wewnątrz funkcji.

[Elvis]

Mozliwe ze odczyt z czujnika nie jest idealny. W kodzie caly czas odczytujesz odleglosc:

    dist = getDistance(); 
   if (dist < 40) 

I gdy tylko dostaniesz wynik < 40 przez nastepne 4s robot wykonuje cofanie/skret. Problem moze wynikac z czestego odczytu czujnika - czasem pojawiaja sie np. bledy czy szumy. Program po takim blednym odczycie natychmiast zaczyna cofanie.

Sprobuj dodac jakas forme filtrowania, np. dodaj licznik, ile razy wykryles dist < 40. Dopiero jesli odczytasz kilka razy ze przeszkoda jest blisko, rozpoczynaj cofanie.

[skmskm]

Dodaj timeout do pulseIn i wszystko powinno zacząć działać.