Problem z programem Line follower z wykrywaniem przeszkód

[Grindas]

Witam ,

buduje pojazd line follower z wykrywaniem przeszkódi mam problem z programem.

Pojazd po uruchomieniu ma najpierw sprawdzić czy nie ma przeszkody w zasięgu 20cm na wprost , po lewej i po prawej stronie pojazdu. Zadanie to jest wykoywane przez czujnik ultradźwiękowy HC - SR04 zamontowany na Serwo TowerPro SG-90. W przypadku braku przeszkody pojazd ma wyszukać linie kręcąc się dookoła , jeśli wykryje linie czujnikiem środkowym ma jechać prosto , następnie jeśli prawy lub lewy czujnik odbiciowy wykryje linie ma naprowadzić pojazd na środek.

Ostatnim warunkiem jest sytuacja w której wszytskie 3 czujniki odbiocowe wykryją linię prostopadłą do trasy pojazdu , ma on się zatrzymać na 5 sekund (taka linia stopu).

Poniżej mój program

Program

Pojazd niestety nie zachowuje się jak należy. Gdy pojazd ustawie na linii trasy i widzi ją tylko środkowym czujnikiem odbiciowym , pojazd zamiast jechać płynnie prosto to , przesuwa się co 1 mm i nie wiem czemu serwo się delikatnie kręci w prawo i w lewo choć serwo ma działać tylko w sytuacji wykrycia przeszkody.

Proszę o porady jak rozwiązać problem.

[Treker (Damian Szymański)]

Grindas, wstaw proszę program w treść swojego postu korzystając z tagów - wszystkim będzie wtedy łatwiej. Możesz to zrobić korzystając z przycisku edytuj przy swojej poprzedniej wiadomości.

Twój kod jest długi i pozbawiony komentarzy, więc ciężko go analizować. Ale spróbujmy... Zacznijmy od tego, czy sprawdziłeś jakie wartości otrzymujesz z czujnika odległości? Może są jakieś zakłócenia, które zaburzają pracę programu? Po drugie, jeśli najważniejszym warunkiem dla Ciebie jest to, czy środkowy czujnik jest nad linią, to może należy napisać program w taki sposób:

Jeśli czujnik_srodkowy = widzi_linie to

jedź_po_linii()

jeśli nie to

dopiero tutaj szukaj przeszkody

Nie zmieni to wiele od strony działania, ale będzie łatwiejsze w późniejszym zrozumieniu... W tej chwili wygląda na to, że z jakiegoś powodu trafiasz regularnie do tego warunku związanego z wykryciem przeszkody. Ciężko jednak analizować Twój program dokładniej przez brak wspomnianych komentarzy 🙁 Obstawiałbym też te problemy z czujnikiem. Jeśli nie masz odpowiednio rozwiązanego zasilania, to ruchy silników mogą generować znaczne spadki napięcia na zasilaniu, co może generować zakłócenia w czujniku. Więc może po prostu robot co chwile widzi tę przeszkodę. Dla testu można też po "forward();" dać opóźnienie np. 100 ms, wtedy będziesz widział co ile obiegów pętli robot zaczyna ruszać serwem.

[Grindas]

Witam ponownie,

zmodyfikowałem lekko mój pojazd i program, lecz nadal mam problem. Pojazd jeździ wzdłuż linii, gdy widzi przeszkodę poniżej 20 cm to się zatrzymuje , ale nie spełnia funkcji zatrzymania pojazdu ,gdy wszystkie trzy czujniki widzą linie (linia stopu prostopadła do trasy). Problem w tym , że wcześniej, gdy pojazd był zaprogramowany na jazdę wzdłuż linii bez wykrywania przeszkód to spełniał tą funkcję bez problemu. W czym może tkwić problem?

Poniżej aktualny program:

#define LT1 digitalRead(11) //czujnik lewy
#define LT2 digitalRead(4) //czujnik środkowy
#define LT3 digitalRead(3) //czujnik prawy

#define ENA 10
#define ENB 5
#define IN1 9
#define IN2 8
#define IN3 7
#define IN4 6
#define Echo A4
#define Trig A5
int middleDistance = 0;

#define ABS 100
void forward(){
 analogWrite(ENA, ABS);
 analogWrite(ENB, ABS);
 digitalWrite(IN1, HIGH);
 digitalWrite(IN2, LOW);
 digitalWrite(IN3, LOW);
 digitalWrite(IN4, HIGH);
 Serial.println("go forward!");
}

void back(){
 analogWrite(ENA, ABS);
 analogWrite(ENB, ABS);
 digitalWrite(IN1, LOW);
 digitalWrite(IN2, HIGH);
 digitalWrite(IN3, HIGH);
 digitalWrite(IN4, LOW);
 Serial.println("go back!");
}

void left(){
 analogWrite(ENA, ABS);
 analogWrite(ENB, ABS);
 digitalWrite(IN1, HIGH);
 digitalWrite(IN2, LOW);
 digitalWrite(IN3, HIGH);
 digitalWrite(IN4, LOW); 
 Serial.println("go left!");
}

void right(){
 analogWrite(ENA, ABS);
 analogWrite(ENB, ABS);
 digitalWrite(IN1, LOW);
 digitalWrite(IN2, HIGH);
 digitalWrite(IN3, LOW);
 digitalWrite(IN4, HIGH);
 Serial.println("go right!");
} 

void stop(){
  digitalWrite(ENA, LOW);
  digitalWrite(ENB, LOW);
  Serial.println("Stop!");
} 

int Distance_test()  //wykrywanie przeszkody
{
 digitalWrite(Trig, LOW);  
 delayMicroseconds(2);
 digitalWrite(Trig, HIGH);  
 delayMicroseconds(20);
 digitalWrite(Trig, LOW);  
 float Fdistance = pulseIn(Echo, HIGH);  
 Fdistance= Fdistance/58;      
 return (int)Fdistance;
}  
void linefollower(){ //podążanie wzdłuż linii
 if(LT2){
   forward();
 }
 else if(LT1) { 
   left();
   while(LT1);                             
 }   
 else if(LT3) {
   right();
   while(LT3);  
 }
 else if(LT1&&LT2&&LT3) { //jesli 3 czujniki widzą linie zatrzymaj pojazd na 5s
   stop();
   delay(5000);
   forward();
}
}
void setup(){
 Serial.begin(9600);
 pinMode(Echo,INPUT);    
 pinMode(Trig,OUTPUT);
}

void loop() {
  middleDistance = Distance_test();
{
if(middleDistance<=20) {
 stop();
}
 else{
   linefollower();
 }
 }
}