Sterowanie robotem za pomocą IR i omijanie przeszkód na Arduino

[DokK2]

Witam,
Próbuję stworzyć robota jeżdżącego za pomocą diody IR. Robot ma na celu omijać przeszkody jeśli podczas jazdy wystąpi. Problem występuje, gdy przesyłam komendę z pilota. Program zapętla się, gdy wcisnę przycisk jakikolwiek, a dokładnie po wciśnięciu przycisku 2 lub 5 już nic nie można zrobić. Na zdjęciu poniżej przedstawiam jak to wygląda.

Poniżej przedstawiam mój kod programu. Dodam, że korzystałem z kodu z kursu budowa robota.

#include <RC5.h>

//Biblioteka od serwomechanizmu
#include <Servo.h>
Servo serwo;
#define SERWO_PIN 11

#define L_PWM 5
#define L_DIR 4
#define R_PWM 6
#define R_DIR 9
#define PWM_MAX 165

#define L_SIDE_SENSOR A1
#define R_SIDE_SENSOR A0 
#define BUZZER 10
#define LED 13
#define TSOP_PIN 3

//Piny od czujnika odleglosci
#define trigPin 8
#define echoPin 7
unsigned char komenda,adres,przelacznik;
RC5 rc5(TSOP_PIN); //Informacja o podłączeniu odbiornika TSOP
unsigned char address; 
unsigned char command;
unsigned char toggle;
long dystans;

void setup() {
 //Konfiguracja pinow od mostka H
 pinMode(L_DIR, OUTPUT);
 pinMode(R_DIR, OUTPUT);
 pinMode(L_PWM, OUTPUT);
 pinMode(R_PWM, OUTPUT);

 //Konfiguracja pozostalych elementow
 pinMode(BUZZER, OUTPUT);
 digitalWrite(BUZZER, 0); //Wylaczenie buzzera    
 pinMode(LED, OUTPUT); 
 digitalWrite(LED, 0); //Wylaczenie diody

 Serial.begin(9600);

 //Konfiguracja pinow od czujnikow
 pinMode(L_SIDE_SENSOR, INPUT_PULLUP);
 pinMode(R_SIDE_SENSOR, INPUT_PULLUP);

 //Czujnik odleglosci
 pinMode(trigPin, OUTPUT); //Pin, do którego podłączymy trig jako wyjście
 pinMode(echoPin, INPUT); //a echo, jako wejście

 //Serwo do pinu 11
 serwo.attach(SERWO_PIN);
 //Serwo na pozycje srodkowa 90 (bo zakres 0-180)
 serwo.write(90);
}

int predkoscObrotu = 30;

void loop() 
{
   odczyt_IR(przelacznik,adres,komenda);
   sterowanie_zdalnie(komenda);
}

int zmierzOdleglosc() 
{
 long czas;

 digitalWrite(trigPin, LOW);
 delayMicroseconds(2);
 digitalWrite(trigPin, HIGH);
 delayMicroseconds(10);
 digitalWrite(trigPin, LOW);

 czas = pulseIn(echoPin, HIGH);
 dystans = czas / 58;

 return dystans;
}

void leftMotor(int V) {
 if (V > 0) { //Jesli predkosc jest wieksza od 0 (dodatnia)
   V = map(V, 0, 100, 0, PWM_MAX);
   digitalWrite(L_DIR, 0); //Kierunek: do przodu
   analogWrite(L_PWM, V); //Ustawienie predkosci 
 } else {
   V = abs(V); //Funkcja abs() zwroci wartosc V  bez znaku
   V = map(V, 0, 100, 0, PWM_MAX);
   digitalWrite(L_DIR, 1); //Kierunek: do tyłu
   analogWrite(L_PWM, V); //Ustawienie predkosci    
 }
}

void rightMotor(int V)
{
   if (V > 0) 
   { //Jesli predkosc jest wieksza od 0 (dodatnia)
       V = map(V, 0, 100, 0, PWM_MAX);
       digitalWrite(R_DIR, 0); //Kierunek: do przodu
       analogWrite(R_PWM, V); //Ustawienie predkosci 
   } 
   else 
   {
       V = abs(V); //Funkcja abs() zwroci wartosc V  bez znaku
       V = map(V, 0, 100, 0, PWM_MAX);
       digitalWrite(R_DIR, 1); //Kierunek: do tyłu
       analogWrite(R_PWM, V); //Ustawienie predkosci    
   }
}

void stopMotors()
{
 analogWrite(L_PWM, 0); //Wylaczenie silnika lewego
 analogWrite(R_PWM, 0); //Wylaczenie silnika prawego
}

void odczyt_IR(unsigned char &pr, unsigned char &a, unsigned char &k)
{
 if(rc5.read(&toggle, &address, &command))
 {
   pr=toggle;
   a=address;
   k=command;
 }
}

void sterowanie_zdalnie(unsigned char &komenda)
{

  Serial.println(komenda);

    switch(komenda) 
    {
     case 2: //Do przodu
           leftMotor(predkoscObrotu);
           rightMotor(predkoscObrotu); //Jesli nie, to jedz prosto
           zmierzOdleglosc();
           Serial.println("Przejechal odleglosc");
           if (dystans<40)
           {
             przeszkoda();
             Serial.println("Jestem w ifie");
           }

           zderzaki();

           Serial.println("Zderzaki");


    break;

     case 5: //STOP
       stopMotors();
       serwo.write(90);
       digitalWrite(BUZZER, 1);
       delay(500);
       digitalWrite(BUZZER, 0);    
       Serial.println("Wykonuje 5");
     break;
    }

}

void przeszkoda()
{
       //Jesli przeszkoda
       stopMotors(); //Zatrzymaj robota
       serwo.write(20); //Skrec czujnikiem w prawo
       delay(1000); //Poczekaj 800ms dla ustabilizowania konstrukcji

       zmierzOdleglosc();
       delay(500);

       if (dystans < 40)
       {
             //Jeśli po prawej jest przeszkoda
             serwo.write(160); //Obroc czujnik w lewo
             delay(800); //Poczekaj 800ms dla ustabilizowania konstrukcji

             zmierzOdleglosc();

             delay(500);


             if(dystans < 40)
             {
                 //Jesli z przodu, z lewej i prawej jest przeszkoda
                 digitalWrite(BUZZER, 1);
                 delay(500);
                 digitalWrite(BUZZER, 0);
                 //Daj sygnal buzzerem

                leftMotor(-predkoscObrotu);
                rightMotor(-predkoscObrotu);
                 delay(500);
                 // do tyłu przez 400 ms
             }
     }
     //Po sprawdzeniu przeszkod po bokach
     //Ustaw czujnik prosto    
     serwo.write(90);


   //Opoznienie 100ms, ponieważ nie ma potrzeby sprawdzać przeszkod czesciej
   delay(100); 
}

void zderzaki()
{
  if (digitalRead(L_SIDE_SENSOR) == LOW) 
   {
     //jazda do tyłu
     leftMotor(-predkoscObrotu);
     rightMotor(-predkoscObrotu);
     delay(800);

     //obrót w miejscu w prawo
     leftMotor(predkoscObrotu);
     rightMotor(-predkoscObrotu);  
     delay(120);
   }

   if (digitalRead(R_SIDE_SENSOR) == LOW) 
   {
     //jazda do tyłu
     leftMotor(-predkoscObrotu);
     rightMotor(-predkoscObrotu);
     delay(800);

     //obrót w miejscu w lewo
    leftMotor(-predkoscObrotu);
    rightMotor(predkoscObrotu);   
     delay(120);
   }
}

Mój mikrokontroler to Arduino Leonardo.

[deshipu]

Nie zerujesz komendy, więc jak już robot jakąkolwiek odbierze, to potem wykonuje ją cały czas.

[DokK2]

Po wstawieniu resetu w funkcji nadal występuje ten problem.

Zrzut ekranu

Reset wstawiam w miejsce funkcji poniżej przedstawiam miejsce.

void odczyt_IR(unsigned char &pr, unsigned char &a, unsigned char &k)
{
 if(rc5.read(&toggle, &address, &command))
 {
   pr=toggle;
   a=address;
   k=command;

   rc5.reset();
 }
}

[deshipu]

Nie zrozumiałeś. Zmienna "komenda" nadal zawiera ostatnią komendę po tym jak już ją wykonałeś.

[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

[DokK2]

W miejsce switcha wstawiłem komende, aby się wyzerowała.

switch(komenda)
    {
     case 2: //Do przodu
           leftMotor(predkoscObrotu);
           rightMotor(predkoscObrotu); //Jesli nie, to jedz prosto
           zmierzOdleglosc();
           Serial.println("Przejechal odleglosc");
           if (dystans<40)
           {
             przeszkoda();
             Serial.println("Jestem w ifie");
           }

           zderzaki();

           Serial.println("Zderzaki");
           komenda=0;           
    break;

     case 5: //STOP
       stopMotors();
       serwo.write(90);
       digitalWrite(BUZZER, 1);
       delay(500);
       digitalWrite(BUZZER, 0);   
       Serial.println("Wykonuje 5");
       komenda=0;
     break;
    }

Teraz wykonuje instrukcję jeden raz i nie mierzy odległości. Dopiero, gdy dłużej wcisnę klawisz 2 to wykonuje wszystkie instrukcję w danym case. Czy jest jakiś sposób na zapętlenie aby wykonywał ciągle instrukcję i miał możliwość przejścia do drugiej?

Próbowałem z pętlą while. Zmieniłem typ funkcji.

bool odczyt_IR(unsigned char &pr, unsigned char &a, unsigned char &k)
{
 if(rc5.read(&toggle, &address, &command))
 {
   pr=toggle;
   a=address;
   k=command;
   return true;
 }
 return false;
}

void sterowanie_zdalnie(unsigned char &komenda)
{
  unsigned char pr,a,k;
  Serial.println(komenda);

    switch(komenda)
    {
     case 2: //Do przodu
        while(!odczyt_IR(pr,a,k))
        {   
           leftMotor(predkoscObrotu);
           rightMotor(predkoscObrotu); //Jesli nie, to jedz prosto
           zmierzOdleglosc();
           Serial.println("Przejechal odleglosc");
           if(dystans<40)
             {
               stopMotors();
               serwo.write(20);  
               Serial.println("Dystans<40");
             }
             Serial.println("Przejechal odleglosc");
             komenda=0;
             if (odczyt_IR(pr,a,k))
             {
               komenda=k;
               Serial.println("Odczyt IR");
               break;
             }

           zderzaki();

           Serial.println("Zderzaki");

        }  
    break;

     case 5: //STOP
       stopMotors();
       serwo.write(90);
       digitalWrite(BUZZER, 1);
       delay(500);
       digitalWrite(BUZZER, 0);   
       Serial.println("Wykonuje 5");
     break;
    }

} 

[deshipu]

Nie do końca rozumiem co próbujesz osiągnąć. Chcesz, żeby wykonywaną komendę dało się przerwać i zmienić na inną?

[DokK2]

Chodzi mi o to żeby robot wykonywał cały czas określoną metodę czy to 2 czy 5 aż do zmiany sygnału na pilocie. Gdy komenda jest wykonywana raz to nie przechodzi do następnej procedury (zmierz odległość, zderzaki) tylko jedzie prosto. Dopiero gdy przyciskam cały czas np. przycisk 2 to wykonuje wszystkie określone procedury, a chciałbym aby robot wykonywał ten proces po 1 przyciśnięciu.

[deshipu]

Ach, czyli jednak ta komenda miała być pamiętana? Bo z twojego pierwszego postu wynika, że "zapętlanie" się ci przeszkadza?

A może odbieranie sygnałów z pilota po prostu nie działa gdy pracują silniki, bo masz za duże zakłócenia?

[DokK2]

Chodzi mi głównie o to aby wykonywał jedną komendę wydaną za pomocą pilota, aż jej nie zmienię. Gdy użyłem while w loop to nie wiem dlaczego w ogóle nie odbierał sygnałów z pilota. Jeśli jest to wina zakłóceń to w jaki sposób można je ominąć, bądź wytłumić?

void loop() 
{
    odczyt_IR(przelacznik,adres,komenda);
    sterowanie_zdalnie(komenda); 
}

void sterowanie_zdalnie(unsigned char &komenda)
{
  unsigned char pr,a,k;
  Serial.println(komenda);

    switch(komenda)
    {
     case 2: //Do przodu
           leftMotor(predkoscObrotu);
           rightMotor(predkoscObrotu); //Jesli nie, to jedz prosto
           zmierzOdleglosc();
           Serial.println("Przejechal odleglosc");
           if(dystans<40)
             {
               stopMotors();
               serwo.write(20); 
               Serial.println("Dystans<40");
             }

           zderzaki();

           Serial.println("Zderzaki");


    break;

     case 5: //STOP
       stopMotors();
       serwo.write(90);
       digitalWrite(BUZZER, 1);
       delay(500);
       digitalWrite(BUZZER, 0);   
       Serial.println("Wykonuje 5");
     break;
    }

}

[deshipu]

Może zanim zaczniesz je omijać i/lub wytłumiać, upewnij się, że to wina zakłóceń. Na przykład odłącz silniki od zasilania i zobacz, czy wtedy działa poprawnie (na podstawie logów z portu szeregowego).

Tak naprawdę, to wydaje mi się że ta biblioteka do pilota działa tylko wtedy, gdy wystarczająco często wołasz jej funkcję "read". To znaczy, że twój program nie powinien zawierać takich rzeczy, jak "delay(500)" które tam masz.