Obstacle - jeżdzi jak chce - prośba o pomoc

[irq7]

Witam,

Po zbudowaniu pierwszego robota z tego forum "Światłolubny", po sukcesie , postanowiłem spróbować z bardziej autonomicznym robotem. Mój wybór padł na robota typu "Obstacle".

Do jego budowy wykorzystałem następujące elementy:

Arduiono Mega2560

dwukanałowy sterownik silników - L298.

Czujnik odległości HC-SR04

Serwomechanizm TowerPro 90MG

Koło + silnik 65x26mm 6V

Arduion zasilam z baterii 9V, siniki, serwomechanizm i mostek z 4x baterie AAA 1,5. Czujnik jest zasilany z arduino.

Po podłączeniu wszystkiego i napisaniu programu (kod poniżej), robot nie jeździ zgodnie z oczekiwaniem.

Założeni jest takie, aby robot mierzył odległość przed sobą. Jeśli odległość jest mniejsza od 20 cm a większa od 10 cm to ma się zatrzymać i wykonać pomiar odległości z lewej i prawej strony.

Po wykonaniu pomiarów sprawdza z której strony ma większą odległość i w tę stronę ma skręcić.

Jeśli z jakiegoś powodu odległość przed robotę będzie mniejsza niż 10 cm do ma się on wycofać.

#include <Servo.h>

//inicjacja biblioteki serwomechanizmu z sesnsorem odleglosci HC-SR04
Servo Eays_SR04;

//definicja pinow sensora odleglosci HC-SR04
#define trigPin 52
#define echoPin 53
//definicja pinow silnikaA
#define IN1Pin 8
#define IN2Pin 9
#define speedPinA 12
//definicja pinow silnikaB
#define IN3Pin 10
#define IN4Pin 11
#define speedPinB 13

 long Fdistance,Ldistance,Rdistance;


void setup(){
 Serial.begin (9600);
 Eays_SR04.attach(5);
 //Pomaranczowy
 pinMode(trigPin, OUTPUT);
 //Zolty
 pinMode(echoPin, INPUT);

 pinMode(IN1Pin, OUTPUT);
 pinMode(IN2Pin, OUTPUT);
 pinMode(speedPinA,OUTPUT);

 pinMode(IN3Pin, OUTPUT);
 pinMode(IN4Pin, OUTPUT);
 pinMode(speedPinB,OUTPUT);
}

 long scan_dist(){
   long duration, distance;
   digitalWrite(trigPin, LOW);
   delayMicroseconds(10);
   digitalWrite(trigPin, HIGH);
   delayMicroseconds(10);
   digitalWrite(trigPin, LOW);
   duration = pulseIn(echoPin, HIGH);
   //return distance = (duration/2) / 29.1;
   return distance = (duration/58);
 }
 void Weels_Stop(){
   //Zatrzymaj silnik A i B
   digitalWrite(IN1Pin, LOW);
   digitalWrite(IN2Pin, LOW);
   digitalWrite(IN3Pin, LOW);
   digitalWrite(IN4Pin, LOW);
 }
 void Weels_Backward(){
   //W stecz silnik A i B
   digitalWrite(IN1Pin, HIGH);
   digitalWrite(IN2Pin, LOW);
   digitalWrite(IN3Pin, LOW);
   digitalWrite(IN4Pin, HIGH);
 }  
 void Weels_Forward(){
   //W przod silnik A i B
   digitalWrite(IN1Pin, LOW);
   digitalWrite(IN2Pin, HIGH);
   digitalWrite(IN3Pin, HIGH);
   digitalWrite(IN4Pin, LOW);
 }
 void TurnLeft(){
   //Skrec w lewo 
   digitalWrite(IN1Pin, LOW);
   digitalWrite(IN2Pin, HIGH);
   digitalWrite(IN3Pin, LOW);
   digitalWrite(IN4Pin, LOW); 
 }
 void TurnRight(){
   //Skrec w prawo  
   digitalWrite(IN1Pin, LOW);
   digitalWrite(IN2Pin, LOW);
   digitalWrite(IN3Pin, HIGH);
   digitalWrite(IN4Pin, LOW);
 }  

void loop() {
 int angle=91;
 Eays_SR04.write(angle);
 Fdistance = scan_dist();

   if (Fdistance <= 20 && Fdistance >= 10 ) {
     Weels_Stop();
     angle=35;
     Eays_SR04.write(angle);
     Ldistance = scan_dist();
     delay(300);


     angle=91;
     Eays_SR04.write(angle);
     delay(300);

     angle=155;
     Eays_SR04.write(angle);
     Rdistance = scan_dist();
     delay(300);


     angle=91;
     Eays_SR04.write(angle);
     delay(300);


     if (Ldistance >= Rdistance){
       TurnLeft();
       delay(450);
     } 

     else if (Ldistance < Rdistance) {  
       TurnRight();
       delay(450);
     }  
   } 

   else if (Fdistance < 10){  
     Weels_Stop();
     delay(400);
     Weels_Backward();
     delay(500);
   }  
   else {
      Weels_Forward();
   }   
}

Niestety robot jeździ jak chce . To znaczy że mając przed sobą wolna przestrzeń 2m nagle zatrzymuje się i cofa lub skręca. Czasami zatrzyma się przed przeszkodą wykona pomiary i skręca w tę stronę z której ma mniej miejsca.

Co zrobić aby robot jeździł zgodnie z algorytmem opisanym powyżej.

Dziękuje za pomoc.

[marek1707]

Rada nr 1:

Podłącz do jednej a potem do drugiej baterii robota zwykły woltomierz. Jeżeli napięcie tej 9V spadnie w czasie jazdy i wykonywania manewrów poniżej 7V lub tej drugiej poniżej 5V, to już masz pierwszy problem: za słabe źródła. Ja na Twoim miejscu baterię 9V wywaliłbym nawet bez mierzenia.

Rada nr 2:

Porzuć swój program w całości i zacznij testować poszczególne elementy robota osobno, pisząc do nich nowe fragmenty kodu. Zacznij od pomiaru odległości. Żadnych silników, żadnego serwa, tylko Ty, czujnik i przeszkody. Testujesz to na wszelkie możliwe sposoby: statycznie i przesuwając klocki i wypisując wyniki np. przez port szeregowy na PC. Jeśli będziesz miał absolutną pewność, że czujnik i obróbka wyników działa a wypisywane liczby są zawsze dobre, przechodzisz do serwa, potem do silników itd. Jeżeli coś idzie nie tak, zatrzymujesz się nad problemem aż go rozwiążesz. Piszesz nowy kod, zmieniasz go, uśredniasz wyniki, zmieniasz funkcje na inne - wiadomo, typowa praca programistyczna. Raczej się nie zdarza, by duży program działał od razu więc nie załamuj rąk tylko zacznij pracować systematycznie. Nikt nie będzie szukał przypadkowych błędów w Twoim kodzie. To Twoje zadanie. Jeśli nie ogarniasz wszystkiego jednocześnie - podziel projekt na mniejsze fragmenty i rozwiązuj jeden problem na raz.

Swoją drogą Arduino Mega do tego zadania do trochę armata na wróbla.

[irq7]

marek1707 dziękuje za odpowiedź.

Nie miałem wczoraj czasu na testy, postaram się dzisiaj coś w tym temacie zrobić.

Przed napisaniem pytania na forum, robiłem następujące testy:

podłączyłem arduino przez USB i sprawdzałem wskazania czujnika ultradźwiękowego.

W 90 % wskazywał poprawne wartości, czasami z jakiegoś powodu nagle wskazywał 9cm pomimo że do przeszkody było z około 190 cm.

Tak wiec jak sugerujesz może to być problem zasilaniem. Proponujesz od razu wyżyci baterię 9V, to jak wg Ciebie najlepiej zasilać arduion.

Zasilając arduino 9V baterią sugerowałem się filmami na youtube'ie.

Dziękuje za wszystkie sugestie

[marek1707]

Acha, czyli wiesz, że czujnik daje złe odczyty ale nic z tym nie zrobiłeś, robot głupieje a Ty na Forum piszesz "Pomóżcie". Ciekawe. Przepis na dalszą pracę już dostałeś. Zaczynasz pisać program od początku tak, by wszystko co w nim zawierasz działało. Dopiero wtedy przechodzisz do następnych kroków - kolejne warstwy musisz opierać na pewności, że niższe funkcje działają poprawnie.

Na Twoim miejscu z samej ciekawości bym tę baterię zmierzył. Zrobiłeś to? Weź sprawy w woje ręce i zacznij działać samodzielnie. Przecież tacy mądrale jak ja w internecie mogą pleść bzdury. Nie wierz we wszystko. Miernik do ręki i już wiesz co się na zasilaniu dzieje. Bateria za mała? No to szukaj większej. 6 paluszków AA w koszyczku, akumulatory, zasilacze do prób statycznych, możliwości jest mnóstwo. Poczytaj z czego zasilane są inne roboty. Jeśli sam zmierzysz swoją baterię i okaże się, że napięcie spada po 10 minutach zabawy to widząc następny projekt w internecie zasilany z tego samego już będziesz wiedział, że to bzdura i ściema. Sieć to nie wyrocznia, raczej zbieranina tanich, prostych, szybkich i w większości kiepskich pomysłów. Wykorzystaj każdą swoją konstrukcję do uzyskania jak największej wiedzy samodzielnie.

[Polecacz 101]

Produkcja i montaż PCB - wybierz sprawdzone PCBWay!
   • Darmowe płytki dla studentów i projektów non-profit
   • Tylko 5$ za 10 prototypów PCB w 24 godziny
   • Usługa projektowania PCB na zlecenie
   • Montaż PCB od 30$ + bezpłatna dostawa i szablony
   • Darmowe narzędzie do podglądu plików Gerber
Zobacz również » Film z fabryki PCBWay