Problem ze sterowaniem serwomechnizmem

[irq7]

Witam,

Mam prośbę o pomoc w zdiagnozowaniu problemu.

Zbudowałem sobie robota z następujący części:

- Arduino mega

- dwukanałowy sterownik silników L298

- czujnik odległości HC-SR04

- Moduł Bluetooth HC-05

- dwóch siników DC

- serwo TowerPro MG90S

W pierwszej wersji robot był bez modułu Bluetooth. Jego zadaniem było samodzielne omijanie przeszkód. Po kilkunastu próbach i zmianach w kodzie, robot w miarę przyzwoicie radził sobie z tym zadaniem.

W drugim etapie dodałem moduł bluetooth, program na komórkę i rozszerzyłem kod robota aby pracował w dwóch trybach:

1. Automatyczny - samodzielnie omijał przeszkody (pierwsza wersja robota)

2. Manualny - sterowanie robotem za pomocą komórki

I niby wszystko działa , robot zmienia tryby pracy, można min sterować z komórki, manualnie sterować z komórki tylko jest jeden problem. Podczas dodawania rozszerzenia funkcjonalności robocik nabawił się tiku nerwowego. Objawia się on tym że czujnik odległości zamontowany na serwomechanizmie porusza się w zakresie od 91 stopni w kierunku 0 stopni.

Jeśli z poniższego kodu usunę sekcję odpowiedzialną za odczytanie wartości z bluetooth'a i sterowanie manualne to czujnik nie drga.

// imoport bibliotek
#include <SoftwareSerial.h>
#include <Servo.h>

//definicja pinow silnikaA
#define IN1Pin 2
#define IN2Pin 3
//#define speedPinA 12
//definicja pinow silnikaB
#define IN3Pin 4
#define IN4Pin 5
//#define speedPinB 13

//definicja pinów detektora odlegosci
#define trigPin 6
#define echoPin 7
Servo Eays_SR04;
SoftwareSerial MySerial(10, 11); // RX, TX defincja pinów do transmisji szeregowej dla BlueTooth
//Definicja zmiennych globalnych 
int BluetoothData, zm2; // zm2 - zmienna do wyznaczania kieronkow jazdy lub stopu
long Fdistance,Ldistance,Rdistance;
String zm1; // zm1 - zmienna okreslajaca czy sterowania odbywa sie recznie czy automatycznie

void setup() {
  // inicjalizacja transmisji szeregowej
  MySerial.begin(9600);
  //Serial.begin(9600);
  //MySerial.println("Bluetooth On please press 0 stop or 10 run forrward or 20 to run right or 30 to left right or 40 run backward ..");
  //Pomaranczowy
 pinMode(trigPin, OUTPUT);
 //Zolty
 pinMode(echoPin, INPUT);
 Eays_SR04.attach(8);

  pinMode(IN1Pin, OUTPUT);
  pinMode(IN2Pin, OUTPUT);
 // pinMode(speedPinA,OUTPUT); - pin niewykorzystany do sterowania prędkością silnika A

  pinMode(IN3Pin, OUTPUT);
  pinMode(IN4Pin, OUTPUT);
  //pinMode(speedPinB,OUTPUT); - pin niewykorzystany do sterowania prędkością silnika B
}

//Funkcja obliczająca odleglosc przeszkody od czujnika 
long scan_dist(int kat){
   long duration, distance;
   digitalWrite(trigPin, LOW);
   delayMicroseconds(10);
   digitalWrite(trigPin, HIGH);
   delayMicroseconds(10);
   digitalWrite(trigPin, LOW);
   duration = pulseIn(echoPin, HIGH);
   return distance = (duration/58);
 }

void Wheels_Stop(){
   //Zatrzymaj silnik A i B
   digitalWrite(IN1Pin, LOW);
   digitalWrite(IN2Pin, LOW);
   digitalWrite(IN3Pin, LOW);
   digitalWrite(IN4Pin, LOW);
 }
 void Wheels_Backward(){
   //W stecz silnik A i B
   digitalWrite(IN1Pin, HIGH);
   digitalWrite(IN2Pin, LOW);
   digitalWrite(IN3Pin, LOW);
   digitalWrite(IN4Pin, HIGH);
 }  
 void Wheels_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() {


   if (MySerial.available()){ //sprawdzenie czy sa jakieś dane Bluetooth
      BluetoothData=MySerial.read(); //pobranie danych z Bluetooth do zmiennej BluetoothData 
      MySerial.println("Bluetooth connected ");
      MySerial.println(BluetoothData); // wyswietenie wartosci danych z Bluetooth na terminalu
     if (BluetoothData=='M' or BluetoothData=='m'){
        zm1=String('M');
      }
      else if (BluetoothData=='A' or BluetoothData=='a'){
        zm1=String('A');

      }
      else {
        zm2=BluetoothData;
      }  

   }

     MySerial.println("Value in zm1 : "); 
     MySerial.println(zm1); 
      if (zm1==String('M')){ 
         //if (MySerial.available()){
         //Wheels_Stop();
         //BluetoothData=MySerial.read();  
         switch(zm2){
            case '0':    
              Wheels_Stop(); 
              MySerial.println("Wheels_Stop ");
              break;
            case '1':
              Wheels_Forward();
              MySerial.println("Wheels_Froward ");
              break;
            case '2':
              TurnRight();
              MySerial.println("TurnRight ");
              break;
            case '3':
              TurnLeft();
              MySerial.println("TurnLeft ");
              break;
            case '4':
              Wheels_Backward();
              MySerial.println("Wheels_Backward ");
              break;

         }
        //} 
      }   
      if (zm1==String('A')){
            MySerial.println("Mode Automatic : "); // wyswietenie wartosci danych z BlueTooth na terminalu
            int angle=91; // przypisanie do zmiennej angle wartosci 91 - detektor odległosci w tej pozycji serwomechanizmu mierzy odleglosc przed soba
             MySerial.println("Angle : ");
             MySerial.println(angle);
             Eays_SR04.write(angle);
            delay(200);
            Fdistance = scan_dist(angle); //pomiar odlegosci przed soba

             MySerial.print("Odleglosc0 : ");
             MySerial.println(Fdistance);

            if (Fdistance <= 20 && Fdistance >= 10 ) { //sprawdzeni czy przeszkoda jest w przedziale 10:20 cm przed robotem
               Weels_Stop(); //Zatrzymanie robota
               angle=35;
               Eays_SR04.write(angle);
               delay(200);
               Ldistance = scan_dist(angle); //Sprawdzenie odlegloscie po lewej stornie
               delay(200);
               angle=91;
               Eays_SR04.write(angle);
               delay(200);
               angle=150;
               Eays_SR04.write(angle);
               delay(200);
               Rdistance = scan_dist(angle);  //Sprawdzenie odlegosci po prawej stronie      
                 if (Rdistance >= Ldistance){     
                   TurnRight();
                   delay(450);
                 }
                 else if (Rdistance < Ldistance){
                   TurnLeft();
                   delay(450);
                 }   
           } 
           else if (Fdistance < 10 && Fdistance >0 ){  //sprawdzeni czy przeszkoda jest w przedziale 0:10 cm przed robotem. Zatrzymnie robota i cofnicie sie. 
             Wheels_Stop();
             delay(400);
             Wheels_Backward();
           }
           else if (Fdistance == 0){//czujnik glupieje jesli odlegosc do przeszkody jest wieksza niz 200cm. Wskazuje wtedy odlegosc 0, 
                                    //wiec jedziemy dalej i w kolejnej iteracji sprawdzamy czy nie zbliżamy sie do przeszkody. Predkosc robota nie jest tak duza aby 
                                    //w podaczas jedej intearcji przejechać 10 cm.
             Wheels_Forward();
           }  
           else {
             Wheels_Forward();
           }

      }  
   // }
  delay(400);
  //Eays_SR04.write(91);

}

Nie mam dużego doświadczenia w pisaniu programów w Arduino i jedyny pomysł jaki przyszedł mi do głowy to zbyt długi czasy pomiędzy którymi podaję wartość kąta 91 stopni (czujnik odległości "patrzy" prosto przed robota ) na pin 8. Dodałem więc " Eays_SR04.write(91);" w każdym "case". Niestety po zmianie serwo prawie cały czas było wychylone na 0 stopni i drgało.

Może ktoś z kolegów wspomoże swoją wiedzą i doświadczeniem.

Pozdrawiam

Irek

[deshipu]

Drobna uwaga, "wheel" pisze się inaczej.

W każdym razie z serwami jest tak, że jak nie wiadomo o co chodzi, to chodzi o zasilanie. Możesz napisać w jaki sposób zasilasz tego robota, a serwo w szczególności?

[marek1707]

Notorycznym problemem jest także sytuacja odwrotna (choć też związana z zasilaniem): głupienie czujnika odległości w przypadku zbyt niskiego lub zakłóconego zasilania. A serwa zupełnie spokojnie robią i jedno i drugie 😐

Ultradźwięki są bardzo kapryśne. Program nie powinien polegać na pojedynczych pomiarach tego czujnika a raczej na średniej (tzw. ucinanej lub filtr medianowy także sprawdzi się dobrze) z kilku "pingów". Można też tak napisać algorytm, by pomiary odległości były robione tylko wtedy gdy serwo nie porusza się na skutek wydania poprzedniej komendy zmiany położenia. Raz wykonana funkcja ustawienia nowego położenia robi to "na stałe" - nie musisz jej wielokrotnie powtarzać, chyba że chcesz płynnie i powoli przejeżdżać przez wszystkie pozycje pośrednie - ale wtedy musisz je wyliczać wraz z płynącym czasem. W przypadku jednorazowej zmiany z N na M serwo zrobi to z pełną dostępną prędkością (ale w niezerowym czasie) wykorzystując bardzo duży prąd z zasilania. Dlatego serw nigdy nie zasilasz z tej samej linii co układy cyfrowe i czujniki.

Pokaż rzeczywisty schemat tego cuda.

Oczywiście nikt nie wyklucza błędów w kodzie, ale z tym sam sobie powinieneś radzić przez pisanie prostszych programów testowych udających, odtwarzających lub wręcz prowokujących trudne sytuacje np. ciągłe machanie serwem z jednoczesnymi pomiarami, gromadzenie np. 100 wyników odległości do stałej przeszkody (np. bez żadnego zdalnego sterowania) a potem wypisanie jakiejś statystyki. Jeśli zauważysz jej pogorszenie lub wręcz załamanie w pewnych warunkach i w korelacji z serwem lub komunikacją - to jest jakiś trop.

Użycie programowego UARTa - wiem że tu jest potrzebny, ale zwracam uwagę - także obciąża procesor. W połączeniu z dziejącą się gdzieś automagicznie w tle generacją sygnału do serwa oraz pomiarami długości impulsów zaczyna być gęsto..

EDIT: Kilka dalszych wyrazów także ma inną pisownię 😥

[Lukaszm]

Użycie programowego UARTa

Hmm w sumie nie wiem czemu jest użyty. Przecież Arduino Mega ma 4 oddzielne moduły UART.

[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

[irq7]

Dziękuję za pytania @deshipu (poprawiłem literówki) i @marek1707 podpowiedzi.

Co do połączeń elektrycznych to moduł sterowania silnikami jak i same silniki są zasilane za pomocą 4 baterii 1,5 V (paluszki).

Natomiast Arduino zasilane jest podczas swobodnego poruszania się za pomocą baterii 9V, natomiast do testowania i wgrywania poprawek w kodzie za pomocą kabla USB.

Bluetooth, czujnik odległości i servo są zasilane z arduino.

Nie mam przy sobie robota, ale wieczorem napiszę które elementy są pod jakie piny wpięte.

@marek1707 co do pisania i testowania kodu programu robię to małymi kroczkami.

Tak jak pisałem pierwsza wersja były bez bluetooth'a, i też powstawał etapami, testy samego czujnika odległości bez serwomechanizmu, testowanie samego działania serwomechanizmu, sklejenie kodu. Tutaj napotkałem problem z pomiarem odległości, okazało się że czujnik mierzy odległość w innym miejscu niż się spodziewałem, dlatego dodałem "delay" przed każdym pomiarem aby mieć pewność że serwomechanizm zdąży ustawić się w odpowiednim położeniu. Potem przyszedł czas na testy "w terenie" i wyskoczyły problemy z pomiarem odległości większej niż 200 cm, itp.

Teraz skończyły mi się pomysły co może powodować takie zachowanie, dlatego tutaj piszę.

Tak jak wspomniałem to jest mój drugi robot. Pierwszy to był światłolubny wg schematu z tego forum. Więc jak widzisz nie mam dużego doświadczenia w tym temacie.

Pozdrawiam

Irek

[marek1707]

Bateria 9V a z niej Arduino, czujnik i serwo? No to masz odpowiedź.

USB i z niego serwo? To masz odpowiedź nr. 2.

Serwo to obciążony silnik - zwierający zasilanie przy starcie i syfiący po zasilaniu jak stara śmieciara - do jego napędzania potrzebujesz minimum paluszków AA w odpowiedniej konfiguracji i osobnej linii. No i zapomnij o bateryjkach 9V użytych do czegoś więcej niż diodka LED lub podobny gadżet na choinkę.

[irq7]

dzięki za wskazanie w którym miejscu mam szukać rozwiązania

Ja chciałem grzebać w kodzie ,a tu problem może być w zasilaniu.

Mam jeszcze trzy pytania.

1. Czy zasilanie serwomechanizmu musi być z osobnego źródła, czy można je zasilać z tego samego co silniki i moduł do ich sterowania?

2. Jeśli do zasilania serwa i silników muszą być osobne źródła, to czy połączone szeregowe 4 paluszki 1,5V (razem da to 4,5V) wystarczy?

3. Czy do zasilania arduino można użyć zasilacza 12V 2A do testów stacjonarnych, a wersji mobilnej akumulatora LiPo 3S 11,1V 2200 mAh?

[deshipu]

1. Można z tego samego, co silniki -- w zasadzie serwo to taki silnik, więc te same zasady obowiązują.

2. 4*1.5 = 6 jak ostatnio sprawdzałem

3. Można. Ale można też zasilać je tymi 6V z paluszków. Fakt, silniki i serwo będą trochę zakłóceń generować, ale da się to odfiltrować, dopóki napięcie na bateriach nie będzie siadać od zbyt wielkiego poboru prądu, to powinno być dobrze.

Rozważ jeszcze użycie akumulatorków NiCd, NiMg albo NiMH w miejsce paluszków. Co prawda mają tylko 1.2V, ale za to zazwyczaj pozwalają na znacznie większy prąd, no i można je ładować.