• Witaj na Forum Arduino Polska! Zapraszamy do rejestracji!
  • Znajdziesz tutaj wiele informacji na temat hardware / software.
Witaj! Logowanie Rejestracja


Ocena wątku:
  • 0 głosów - średnia: 0
  • 1
  • 2
  • 3
  • 4
  • 5
Odtwarzanie trasy przejazdu robota - poprawność programu
#1
Lightbulb 
Witam. Jestem nowy na forum, więc dzień dobry Smile

Jestem w trakcie realizacji projektu 2-kołowego robota mobilnego, którego celem jest zapamiętywanie trasy przejazdu, a następnie jej odtworzenie. Sterowanie pojazdem odbywa się za pomocą aplikacji na telefonie z Androidem napisanej w środowisku online MIT App Inventor 2. Komunikacja z robotem realizowana jest poprzez Bluetooth (HC-06). Pojazd napędzany jest dwoma silnikami z przekładniami, umieszczonymi na podwoziu Romi Chassis (Pololu). Na każdym silniku umieszczony jest enkoder o rozdzielczości 1440 imp/obr. Kanały A i B każdego enkodera połączone są z wejściami bramki XOR, dzięki czemu na wyjściu mamy jeden sygnał o podwójnej częstotliwości. Dzięki temu przy wykorzystaniu tylko 2 przerwań zewnętrznych w Arduino Leonardo możemy obsłużyć z pełną rozdzielczością dwa enkodery (kierunek obrotów nie jest ważny w przypadku tego projektu). Trasa zapamiętywana jest w tablicy 4-kolumnowej, gdzie każda kolumna odpowiada odpowiednio za drogę do przodu/tyłu/lewo/prawo. Wartość drogi, to ilość impulsów zliczonych z enkoderów. 

Napisany przeze mnie program w środowisku Arduino IDE 1.8.7. działa następująco:
1. Pomiar napięcia i wysłanie wartości odczytu "do telefonu" (wartość wyświetlana na ekranie aplikacji)
2. Kontrola wartości napięcia baterii (zbyt niskie napięcie zatrzymuje pojazd i włącza alarm)
3. Odczytanie danych z UART (Bluetooth), po czym wykonywane są konkretne czynności zależnie od otrzymanego znaku z aplikacji na Androidzie. 

Protokół komunikacyjny jest czytelnie opisany w komentarzach programu.

Sterowanie silnikami odbywa się z wykorzystaniem regulatora PD w celu zrównania prędkości obrotowej dwóch silników, niezależnie od obciążenia. Dzięki temu można także odtwarzać drogę na podstawie zapisanych odczytów z tylko jednego enkodera. 
Skręcanie możliwe jest tylko w miejscu (obrót).

Problemem, który napotkałem jest dziwne zachowanie podczas odtwarzania trasy przejazdu. Dodam, że początkowo wszystko działało prawidłowo na tym sketchu, jednak teraz jest inaczej. 
Konkretnie chodzi o wiersz 100 programu ( i analogicznie wiersz 108, 116, 124). Bez wpisania w tym miejscu funkcji wysyłania czegoś transmisją szeregową, program nie opuszcza pętli while. Gdy ją wpiszę, wszystko działa prawie prawidłowo.
Prawie, ponieważ napotkałem jeszcze na jeden problem. Podczas gdy obracam pojazd w lewo o np. kąt 90 stopni, to podczas powrotu robot obraca się o mniejszy kąt (około 70 stopni), natomiast gdy obracam go w prawo o kąt 90 stopni, to podczas powrotu obraca się o większy kąt (około 110 stopni). Pomiary za każdym razem wychodzą takie same. Zależy mi na jak największej dokładności, ponieważ przy skomplikowanej trasie (dużo zakrętów, długie trasy na wprost) błędy sumują się i ostatecznie robot nigdy nie wróci do miejsca z którego wystartował.
Nie wiem co może być przyczyną takiego zachowania. Może czasy wykonywania jakichś fragmentów kodu? Męczę się z tym już bardzo długo i nic nowego nie wnoszę do tego projektu. Bardzo proszę o pomoc.
Poniżej zamieszczam program wraz z komentarzami, screenshot z aplikacji na arduino, zdjęcia robota oraz schemat elektroniki zrobionej przeze mnie w celu sterowania silnikami, komunikacji z HC-06, alarmu itp.

Kod:
#define B_ENC 7 //enkoder silnika A
#define A_ENC 3 //enkoder silnika B
#define STDBY 4 //uśpienie sterownika silników
#define ALR 5 //sygnał alarmu
#define A_IN1 2 //zmiana kierunku obrotów silnika prawego
#define A_IN2 6 //
#define A_PWM 9 //wyjście PWM na silnik prawy
#define B_IN1 8 //zmiana kierunku obrotów silnika prawego
#define B_IN2 11 //
#define B_PWM 10 //wyjście PWM na silnik lewy
int A = 0; // impulsy enkodera silnika A
int B = 0; // impulsy enkodera silnika B
int line = 0; //numer wiersza macierzy (krok wykonania ruchu powrotnego)
int macierz [40][4] = {}; //tablica danych trasy przejazdu
int Speed1 = 120; //prędkość obrotów silników w trybie normalnym
int Speed2 = 65; //prędkość obrotów silników w trybie nauki
int Speed3 = 65; //prędkość obrotów silników w trybie powrotu
int w_zad = 0; //wartość zadana
int k_p = 3; //współczynnik wzmocnienia członu P regulatora
int k_d = 1; //stała różniczkowania regulatora
int p; //wartość wyjściowa członu typu P
int d; //wartość wyjściowa członu typu D
int u; //aktualny uchyb regulacji
int u_p = 0; //poprzedni uchyb regulacji
int out; // sygnał wyjściowy regulatora
int diff = 0; //różnica wskazań enkoderów
int x; // regulowana wartość PWM A
int y; // regulowana wartość PWM B
boolean Forward = 0; //znacznik jazdy do przodu
boolean Backward = 0; //znacznik jazdy do tyłu
boolean Left = 0; //znacznik jazdy w lewo
boolean Right = 0; //znacznik jazdy w prawo
char state = 0; //odczyt z bluetooth(SerialPort1)
int meas = 0; // wartość odczytana z ADC
float volt; // obliczona wartość napięcia

void setup() {

  pinMode(A_ENC, INPUT_PULLUP); //kofiguracja wejść i wyjść
  pinMode(B_ENC, INPUT_PULLUP);
  pinMode(ALR, OUTPUT);
  pinMode(STDBY, OUTPUT);
  pinMode(A_IN1, OUTPUT);
  pinMode(A_IN2, OUTPUT);
  pinMode(A_PWM, OUTPUT);
  pinMode(B_IN1, OUTPUT);
  pinMode(B_IN2, OUTPUT);
  pinMode(B_PWM, OUTPUT);
  Serial1.begin(9600); //włączenie transmisji szeregowej i ustawienie prędkości
  Serial.begin(9600);
  attachInterrupt(digitalPinToInterrupt(A_ENC), COUNT_A, CHANGE); // włączenie przerwań zewnętrznych
  attachInterrupt(digitalPinToInterrupt(B_ENC), COUNT_B, CHANGE);
}
void loop() {
  
  meas = analogRead(A0);   // pomiar napięcia baterii 
  volt = meas * (0.0097752);  // przeliczenie na wartość dziesiętną
  Serial1.print(" "); // wysłanie wartości przez bluetooth
  Serial1.print(volt); //
  
  while (volt < 5.0){ //kontrola napięcia
    digitalWrite(ALR, !digitalRead(ALR)); // napięcie poniżej 5V włącza alarm
    Stop();
    delay(500);
  }
  digitalWrite(ALR, LOW); // wyłączenie alarmu
  
  if (state == '0') { // czyszczenie odczytów z enkoderów
    clearenc();
  } 
  if (Serial1.available()) { // odczyt danych z bluetooth
    state = Serial1.read(); // przypisanie odebranej danej do zmiennej
  }
  switch (state) {
    case '0': //stop
      Stop();
      clearenc();
      break;
    case '1': //
      forward(Speed1); //do przodu
      break;
    case '2':
      backward(Speed1); //do tyłu
      break;
    case '3':
      left(Speed1); //w lewo
      break;
    case '4':
      right(Speed1); //w prawo
      break;
    case '5': // nauka
      clearenc();
      break;
    case '6':                          // powrót
      for (int i = 39; i >= 0; i--) {
        clearenc();
        if (macierz[i][0] > 0) {
          while (A < macierz[i][0]) { //do tyłu
            backward(Speed3);
            Serial.println(A); // bez tego wiersza program  nie opuszcza pętli while. (???)
          }
          Stop();
          macierz[i][0] = 0;
        }
        else if (macierz[i][1] > 0) { // do przodu
          while (A < macierz[i][1]) {
            forward(Speed3);
            Serial.println(A);
          }
          Stop();
          macierz[i][1] = 0;
        }
        else if (macierz[i][2] > 0) { // w prawo
          while (A < macierz[i][2]) {
            right(Speed3);
            Serial.println(A);
          }
          Stop();
          macierz[i][2] = 0;
        }
        else if (macierz[i][3] > 0) { // w lewo
          while (A < macierz[i][3]) {
            left(Speed3);
            Serial.println(A);
          }
          Stop();
          macierz[i][3] = 0;
        }
      }
      line = 0;
      break;
    case '7': //do przodu w trybie nauki
      forward(Speed2);
      Forward = 1;
      break;
    case '8': //do tyłu w trybie nauki
      backward(Speed2);
      Backward = 1;
      break;
    case '9': //w lewo w trybie nauki
      left(Speed2);
      Left = 1;
      break;
    case 'A': //w prawo w trybie nauki
      right(Speed2);
      Right = 1;
      break;
    case 'B':
      Stop(); // zatrzymanie w trybie nauki + zapamiętanie przejechanej trasy
      if (Forward == 1) { //zapisanie drogi do przodu
        macierz [line] [0] = A;
      }
      else if (Backward == 1) { //zapisanie drogi do tyłu
        macierz [line] [1] = A;
      }
      else if (Left == 1) { //zapisanie obrotu w lewo
        macierz [line] [2] = A;
      }
      else if (Right == 1) { //zapisanie obrotu w prawo
        macierz [line] [3] = A;
      }
      line ++;
      Forward = 0;
      Backward = 0;
      Left = 0;
      Right = 0;
      clearenc();
      break;
  }
}
void COUNT_A() { //zliczanie impulsów ennkodera silnika A
  A++;  
}  
void COUNT_B() { //zliczanie impulsów ennkodera silnika B
  B++;
}
void forward(int Speed) { //funkcja jazdy do przodu + synchronizacja prędkości obrotowej silników
  diff = A - B;
  u = w_zad - diff;
  p = k_p * u;
  d = k_d * (u_p - u);
  out = p + d;
  u_p = u;
  x = Speed - out;
  y = Speed + out;
  if(x > 255){
    x = 255;
  }else if(x < 35){
    x = 0;
  }
  if(y > 255){
    y = 255;
  }else if(y < 35){
    y = 0;
  }
  digitalWrite(A_IN1, HIGH); 
  digitalWrite(A_IN2, LOW);
  digitalWrite(B_IN1, LOW); 
  digitalWrite(B_IN2, HIGH);
  analogWrite(B_PWM, x);
  analogWrite(A_PWM, y);
  digitalWrite(STDBY, HIGH);
}
void backward(int Speed) { //funkcja jazdy do tyłu + synchronizacja prędkości obrotowej silników
  diff = A - B;
  u = w_zad - diff;
  p = k_p * u;
  d = k_d * (u_p - u);
  out = p + d;
  u_p = u;
  x = Speed - out;
  y = Speed + out;
  if(x > 255){
    x = 255;
  }else if(x < 35){
    x = 0;
  }
  if(y > 255){
    y = 255;
  }else if(y < 35){
    y = 0;
  }
  digitalWrite(A_IN1, LOW); 
  digitalWrite(A_IN2, HIGH);
  digitalWrite(B_IN1, HIGH); 
  digitalWrite(B_IN2, LOW);
  analogWrite(B_PWM, x);
  analogWrite(A_PWM, y);
  digitalWrite(STDBY, HIGH);
}
void left(int Speed) { //funkcja jazdy w lewo  + synchronizacja prędkości obrotowej silników
  diff = A - B;
  u = w_zad - diff;
  p = k_p * u;
  d = k_d * (u_p - u);
  out = p + d;
  u_p = u;
  x = Speed - out;
  y = Speed + out;
  if(x > 255){
    x = 255;
  }else if(x < 35){
    x = 0;
  }
  if(y > 255){
    y = 255;
  }else if(y < 35){
    y = 0;
  }
  digitalWrite(A_IN1, HIGH); 
  digitalWrite(A_IN2, LOW);
  digitalWrite(B_IN1, HIGH); 
  digitalWrite(B_IN2, LOW);
  analogWrite(B_PWM, x);
  analogWrite(A_PWM, y);
  digitalWrite(STDBY, HIGH);
  
}
void right(int Speed) { //funkcja jazdy w prawo  + synchronizacja prędkości obrotowej silników
  diff = A - B;
  u = w_zad - diff;
  p = k_p * u;
  d = k_d * (u_p - u);
  out = p + d;
  u_p = u;
  x = Speed - out;
  y = Speed + out;
  if(x > 255){
    x = 255;
  }else if(x < 35){
    x = 0;
  }
  if(y > 255){
    y = 255;
  }else if(y < 35){
    y = 0;
  }
  digitalWrite(A_IN1, LOW); 
  digitalWrite(A_IN2, HIGH);
  digitalWrite(B_IN1, LOW); 
  digitalWrite(B_IN2, HIGH);
  analogWrite(B_PWM, x);
  analogWrite(A_PWM, y);
  digitalWrite(STDBY, HIGH);
}
void Stop() {
  digitalWrite(A_IN1, HIGH); // stop
  digitalWrite(A_IN2, HIGH);
  digitalWrite(B_IN1, HIGH); 
  digitalWrite(B_IN2, HIGH);
  analogWrite(A_PWM, 0);
  analogWrite(B_PWM, 0);
  delay(100);
  digitalWrite(STDBY, LOW);
}
void clearenc() { //funkcja czyszczenia odczytów z enkoderów
  A = 0;
  B = 0;
}

[Obrazek: IMG-20181125-222244.jpg]
[Obrazek: IMG-20181125-222303.jpg]
[Obrazek: IMG-20181125-222338.jpg]
[Obrazek: Screenshot-20181125-221426.png]

P.S. Z chęcią przyjmę uwagi odnoście "stylu" całego programu i sugestie jego upiększenia, jak zapewne zastosowanie zmiennych lokalnych, a nie tylko globalnych Smile


Załączone pliki
.pdf   PCB_pojazd_samobiezny.pdf (Rozmiar: 61.75 KB / Pobrań: 6)
 
Odpowiedź
#2
Zmienne A  i  B (zmiennym globalnym nadawaj dłuższe nazwy, bo ArduinoIDE to szmelc, który nie potrafi tego co KEIL czy Atolic i w ogóle Arduino, nie powinno być nazywane IDE tak jak i Windows systemem. Szukanie krótkich nazw zmiennych to katastrofa. Spróbuj wyszukać przez CTFL+F wystąpienia zmiennej "A".) używasz zarówno w przerwaniu jak i programie głównym.  Nie poinformowałeś o tym kompilatora.

Deklarujesz zmienne 16 bit
Kod:
for (int i = 39; i >= 0; i--)
a wystarczyło by 8-bit. AVR jest za szybki? Nie sądzę.
W ARM miałoby to sens. Operacje na zmiennych 32-bit wykonują się szybciej niż na 8-bit. W ARM typ int jest 32-bit a nie jak w przestarzałych AVR czy Intel286, 386 16-bit. W takiej sytuacji bezpieczniej deklarować taką zmienną jako int_fast8_t. Zależne od CPU zostanie użyty odpowiednia długość słowa, np w 64-bitowym CPU będą to 64-bity, ARM - 32-bit, AVR - 8-bit, MC68000/08/10 8-bit (68008 na pewno 8-bit, 00/10 może i 16-bit) a pewnie w MC68020/30/40/60 16 lub 32-bit. a może tylko 8-bit,


Co do
Kod:
Serial.println(A); // bez tego wiersza program  nie opuszcza pętli while. (???)
Serial.println działa jak delay.

AVR to nie ARM czy PC. Musisz nauczyć się oszczędzać RAM. Deklarujesz
Kod:
int macierz [40][4]
zmienne 16-bit, 40*4 elementów co pochłania 320bajtów RAM. Z pobieżnej analizy kodu wnioskuję, że wystarczy 8-bit, co pochłonie tylko 160bajtów. Dalej, po co aż 4 elementy na kierunki przód, tył, lewo, prawo?  Użyłeś wykluczeń
Kod:
else if (macierz[i][1] > 0) { // do przodu
co oznacza, że analizowany jest jeden z czterech elementów. Nie lepiej w jednym elemencie zapisać informację o wartości a w drugim o kierunku? Zużycie RAM zmniejszy się dwukrotnie. Zamiast 160bajtów po zaoszczędzeniu na deklaracji 8-bit a nie 16-bit zużyjesz 80 bajtów RAM. 4 razy mniej RAM! Nie analizowałem jakie wartości zapisujesz do tablicy ale może wystarczy  6 bitów, w takiej sytuacji, na pozostałych 2 kodujesz kierunek. Zużyjesz tylko 40 bajtów RAM. 8 razy mniej niż pierwotnie!

Napięcie baterii odczytujesz bez uśredniania. jednorazowy skok napięcia i wszystko się zatrzyma. Tak ma być? Raczej nie!


PS
Generalnie, Arduinowcy, nagminnie używają typów 16-bit tam gdzie wystarczy 8. Dlaczego nie wiem?
W deklaracjach PIO, używają zmiennych 16-bit. Po co skoro w 99% przypadkach mogą to być 8-bit stałe w FLASH a jeszcze lepiej, tak jak zrobiłeś, definicje. To chyba pierwszy kod na Arduino, w którym deklaracje PIO zrobiono z głową!
 
Odpowiedź
#3
Dziękuję bardzo za te wskazówki.
Zastosowałem się do nich i aktualnie kod wygląda następująco:

Kod:
#define B_ENC 7 //enkoder silnika A
#define A_ENC 3 //enkoder silnika B
#define STDBY 4 //uśpienie sterownika silników
#define ALR 5 //sygnał alarmu
#define A_IN1 2 //zmiana kierunku obrotów silnika prawego
#define A_IN2 6 //
#define A_PWM 9 //wyjście PWM na silnik prawy
#define B_IN1 8 //zmiana kierunku obrotów silnika prawego
#define B_IN2 11 //
#define B_PWM 10 //wyjście PWM na silnik lewy
volatile int A_enc = 0; // impulsy enkodera silnika A
volatile int B_enc = 0; // impulsy enkodera silnika B
int line = 0; //numer wiersza macierzy (krok wykonania ruchu powrotnego)
unsigned int macierz [40][4] = {}; //tablica danych trasy przejazdu
int Speed1 = 120; //prędkość obrotów silników w trybie normalnym
int Speed2 = 65; //prędkość obrotów silników w trybie nauki
int Speed3 = 65; //prędkość obrotów silników w trybie powrotu
const int w_zad = 0; //wartość zadana
const int k_p = 3; //współczynnik wzmocnienia członu P regulatora
const int k_d = 1; //stała różniczkowania regulatora
volatile int p; //wartość wyjściowa członu typu P
volatile int d; //wartość wyjściowa członu typu D
volatile int u; //aktualny uchyb regulacji
volatile int u_p = 0; //poprzedni uchyb regulacji
volatile int out; // sygnał wyjściowy regulatora
volatile int diff = 0; //różnica wskazań enkoderów
volatile int x; // regulowana wartość PWM A
volatile int y; // regulowana wartość PWM B
boolean Forward = 0; //znacznik jazdy do przodu
boolean Backward = 0; //znacznik jazdy do tyłu
boolean Left = 0; //znacznik jazdy w lewo
boolean Right = 0; //znacznik jazdy w prawo
char state = 0; //odczyt z bluetooth(SerialPort1)
int meas = 0; // wartość odczytana z ADC
float volt; // obliczona wartość napięcia

void setup() {

  pinMode(A_ENC, INPUT_PULLUP); //kofiguracja wejść i wyjść
  pinMode(B_ENC, INPUT_PULLUP);
  pinMode(ALR, OUTPUT);
  pinMode(STDBY, OUTPUT);
  pinMode(A_IN1, OUTPUT);
  pinMode(A_IN2, OUTPUT);
  pinMode(A_PWM, OUTPUT);
  pinMode(B_IN1, OUTPUT);
  pinMode(B_IN2, OUTPUT);
  pinMode(B_PWM, OUTPUT);
  Serial1.begin(9600); //włączenie transmisji szeregowej i ustawienie prędkości
  Serial.begin(9600);
  attachInterrupt(digitalPinToInterrupt(A_ENC), COUNT_A, CHANGE); // włączenie przerwań zewnętrznych
  attachInterrupt(digitalPinToInterrupt(B_ENC), COUNT_B, CHANGE);
}
void loop() {
  
  meas = analogRead(A0);   // pomiar napięcia baterii
  volt = meas * (0.0097752);  // przeliczenie na wartość dziesiętną
  Serial1.print(" "); // wysłanie wartości przez bluetooth
  Serial1.print(volt); //
  
  while (volt < 5.0){ //kontrola napięcia
    digitalWrite(ALR, !digitalRead(ALR)); // napięcie poniżej 5V włącza alarm
    Stop();
    delay(500);
  }
  digitalWrite(ALR, LOW); // wyłączenie alarmu
  
  if (state == '0') { // czyszczenie odczytów z enkoderów
    clearenc();
  }
  if (Serial1.available()) { // odczyt danych z bluetooth
    state = Serial1.read(); // przypisanie odebranej danej do zmiennej
  }
  switch (state) {
    case '0': //stop
      Stop();
      clearenc();
      break;
    case '1': //
      forward(Speed1); //do przodu
      break;
    case '2':
      backward(Speed1); //do tyłu
      break;
    case '3':
      left(Speed1); //w lewo
      break;
    case '4':
      right(Speed1); //w prawo
      break;
    case '5': // nauka
      clearenc();
      break;
    case '6':                          // powrót
      for (char  i = 39; i >= 0; i--) {
        clearenc();
        if (macierz[i][0] > 0) {
          while (A_enc < macierz[i][0]) { //do tyłu
            backward(Speed3);
          }
          Stop();
          macierz[i][0] = 0;
        }
        else if (macierz[i][1] > 0) { // do przodu
          while (A_enc < macierz[i][1]) {
            forward(Speed3);
          }
          Stop();
          macierz[i][1] = 0;
        }
        else if (macierz[i][2] > 0) { // w prawo
          while (A_enc < macierz[i][2]) {
            right(Speed3);
          }
          Stop();
          macierz[i][2] = 0;
        }
        else if (macierz[i][3] > 0) { // w lewo
          while (A_enc < macierz[i][3]) {
            left(Speed3);
          }
          Stop();
          macierz[i][3] = 0;
        }
      }
      line = 0;
      break;
    case '7': //do przodu w trybie nauki
      forward(Speed2);
      Forward = 1;
      break;
    case '8': //do tyłu w trybie nauki
      backward(Speed2);
      Backward = 1;
      break;
    case '9': //w lewo w trybie nauki
      left(Speed2);
      Left = 1;
      break;
    case 'A': //w prawo w trybie nauki
      right(Speed2);
      Right = 1;
      break;
    case 'B':
      Stop(); // zatrzymanie w trybie nauki + zapamiętanie przejechanej trasy
      if (Forward == 1) { //zapisanie drogi do przodu
        macierz [line] [0] = A_enc;
      }
      else if (Backward == 1) { //zapisanie drogi do tyłu
        macierz [line] [1] = A_enc;
      }
      else if (Left == 1) { //zapisanie obrotu w lewo
        macierz [line] [2] = A_enc;
      }
      else if (Right == 1) { //zapisanie obrotu w prawo
        macierz [line] [3] = A_enc;
      }
      line ++;
      Forward = 0;
      Backward = 0;
      Left = 0;
      Right = 0;
      clearenc();
      break;
  }
}
void COUNT_A() { //zliczanie impulsów ennkodera silnika A
  A_enc++;  
}  
void COUNT_B() { //zliczanie impulsów ennkodera silnika B
  B_enc++;
}
void forward(int Speed) { //funkcja jazdy do przodu + synchronizacja prędkości obrotowej silników
  diff = A_enc - B_enc;
  u = w_zad - diff;
  p = k_p * u;
  d = k_d * (u_p - u);
  out = p + d;
  u_p = u;
  x = Speed - out;
  y = Speed + out;
  if(x > 255){
    x = 255;
  }else if(x < 35){
    x = 0;
  }
  if(y > 255){
    y = 255;
  }else if(y < 35){
    y = 0;
  }
  digitalWrite(A_IN1, HIGH);
  digitalWrite(A_IN2, LOW);
  digitalWrite(B_IN1, LOW);
  digitalWrite(B_IN2, HIGH);
  analogWrite(B_PWM, x);
  analogWrite(A_PWM, y);
  digitalWrite(STDBY, HIGH);
}
void backward(int Speed) { //funkcja jazdy do tyłu + synchronizacja prędkości obrotowej silników
  diff = A_enc - B_enc;
  u = w_zad - diff;
  p = k_p * u;
  d = k_d * (u_p - u);
  out = p + d;
  u_p = u;
  x = Speed - out;
  y = Speed + out;
  if(x > 255){
    x = 255;
  }else if(x < 35){
    x = 0;
  }
  if(y > 255){
    y = 255;
  }else if(y < 35){
    y = 0;
  }
  digitalWrite(A_IN1, LOW);
  digitalWrite(A_IN2, HIGH);
  digitalWrite(B_IN1, HIGH);
  digitalWrite(B_IN2, LOW);
  analogWrite(B_PWM, x);
  analogWrite(A_PWM, y);
  digitalWrite(STDBY, HIGH);
}
void left(int Speed) { //funkcja jazdy w lewo  + synchronizacja prędkości obrotowej silników
  diff = A_enc - B_enc;
  u = w_zad - diff;
  p = k_p * u;
  d = k_d * (u_p - u);
  out = p + d;
  u_p = u;
  x = Speed - out;
  y = Speed + out;
  if(x > 255){
    x = 255;
  }else if(x < 35){
    x = 0;
  }
  if(y > 255){
    y = 255;
  }else if(y < 35){
    y = 0;
  }
  digitalWrite(A_IN1, HIGH);
  digitalWrite(A_IN2, LOW);
  digitalWrite(B_IN1, HIGH);
  digitalWrite(B_IN2, LOW);
  analogWrite(B_PWM, x);
  analogWrite(A_PWM, y);
  digitalWrite(STDBY, HIGH);
  
}
void right(int Speed) { //funkcja jazdy w prawo  + synchronizacja prędkości obrotowej silników
  diff = A_enc - B_enc;
  u = w_zad - diff;
  p = k_p * u;
  d = k_d * (u_p - u);
  out = p + d;
  u_p = u;
  x = Speed - out;
  y = Speed + out;
  if(x > 255){
    x = 255;
  }else if(x < 35){
    x = 0;
  }
  if(y > 255){
    y = 255;
  }else if(y < 35){
    y = 0;
  }
  digitalWrite(A_IN1, LOW);
  digitalWrite(A_IN2, HIGH);
  digitalWrite(B_IN1, LOW);
  digitalWrite(B_IN2, HIGH);
  analogWrite(B_PWM, x);
  analogWrite(A_PWM, y);
  digitalWrite(STDBY, HIGH);
}
void Stop() {
  digitalWrite(A_IN1, HIGH); // stop
  digitalWrite(A_IN2, HIGH);
  digitalWrite(B_IN1, HIGH);
  digitalWrite(B_IN2, HIGH);
  analogWrite(A_PWM, 0);
  analogWrite(B_PWM, 0);
  delay(100);
  digitalWrite(STDBY, LOW);
}
void clearenc() { //funkcja czyszczenia odczytów z enkoderów
  A_enc = 0;
  B_enc = 0;
}

Udało się zlikwidować problem wyjścia z pętli while. Powodem było operowanie przez kompilator zawartością rejestru pamięci, zamiast, zamiast odwoływać się do zawartości komórki pamięci (optymalizacja). Zastosowanie specyfikatora volatile do zmiennych A i B rozwiązało problem. Dodatkowo wydłużyłem ich nazwy do A_enc i B_enc, dzięki czemu teraz można je łatwiej wyszczególnić poprzez ctrl+f.
Jeśli chodzi o oszczędzanie pamięci RAM, to zwróciłem na to uwagę, jednak tablicy "macierz" specjalnie przypisałem typ 16-bit int, ponieważ zapisywane są w niej wartości zliczonych impulsów enkoderów, a więc są to duże wartości przy rozdzielczości 1440 imp/obr. Dodałem nawet znacznik unsigned, aby wydłużyć zakres do 65535, ponieważ wartości te zawsze przyjmują wartości dodatnie. Może okazać się nawet, że przy dłuższych przejechanych odcinkach wartość do zapisania będzie większa niż zakres uint16_t, więc będzie trzeba zastosować inny typ. Oczywiście wszystko w miarę rozsądku, aby zmieścić się w ilości dostępnej pamięci.
Aktualnie sprawa z pamięcią wygląda tak:
"Szkic używa 9852 bajtów (34%) pamięci programu. Maksimum to 28672 bajtów.
Zmienne globalne używają 699 bajtów (27%) pamięci dynamicznej, pozostawiając 1861 bajtów dla zmiennych lokalnych. Maksimum to 2560 bajtów."
Wracając do problemu błędnego odtwarzania drogi powrotu, to podejrzewam jedną przyczynę:
Impulsy z enkoderów są zliczane w podprogramach przerwać zewnętrznych. W danym momencie może wykonywać się tylko jeden podprogram od jednego przerwania, ponieważ na czas procedury obsługi przerwania procesor wyłącza możliwość wystąpienia innych przerwań. Być może poszczególne zbocza z dwóch enkoderów nakładają się w czasie lub są oddalone od siebie o czas krótszy niż czas wykonania procedury przerwania i wtedy zliczenie impulsu z drugiego enkodera zostaje pominięte? Tylko jak taką sytuację wyeliminować?

P.S. Uśrednianie pomiaru napięcia wykonam w późniejszym czasie, ponieważ nie jest to dla mnie aktualnie priorytetem. Jednak na pewno to poprawię w swoim czasie Smile
 
Odpowiedź
#4
(06-01-2019, 15:33)BourbonKid napisał(a): Udało się zlikwidować problem wyjścia z pętli while. Powodem było operowanie przez kompilator zawartością rejestru pamięci, zamiast, zamiast odwoływać się do zawartości komórki pamięci (optymalizacja). Zastosowanie specyfikatora volatile do zmiennych A i B rozwiązało problem.
Takiej osobie to aż miło pomagać! Dałem wskazówki, poczytałeś, zrozumiałeś, wdrożyłeś, problem rozwiązany. Inni wyzywają mnie od durniów, narzekają, że nie chcę pomóc. Trzeba umieć odróżnić pomoc od zrobienia za kogoś roboty. U mnie koszt pomocy = 0zł, zrobienie roboty 100..200zł za godzinę (kilka razy taniej niż wypisanie recepty przez lekarza).

(06-01-2019, 15:33)BourbonKid napisał(a): Wracając do problemu błędnego odtwarzania drogi powrotu, to podejrzewam jedną przyczynę:
Impulsy z enkoderów są zliczane w podprogramach przerwać zewnętrznych. W  danym momencie może wykonywać się tylko jeden podprogram od jednego przerwania, ponieważ na czas procedury obsługi przerwania procesor wyłącza możliwość wystąpienia innych przerwań. Być może poszczególne zbocza z dwóch enkoderów nakładają się w czasie lub są oddalone od siebie o czas krótszy niż czas wykonania procedury przerwania i wtedy zliczenie impulsu z drugiego enkodera zostaje pominięte? Tylko jak taką sytuację wyeliminować?

Wracając do meritum sprawy. Jaka jest max częstotliwości przerwań z enkoderów? Zależnie od tego, jak jest napisana procedura (musiałbyś zobaczyc rozwinięcie ASM) INT można zliczać 20..50000tyś razy na sekundę (mierzyć 20..50kHz). Jak przerwania wystąpią równocześnie, to jedno z nich będzie wykonane później. W AVR można pozwolić na przerwanie w przerwaniu (deklaracja przerwania ISR_NOBLOCK albo INTERRUPT). Jeśli faktycznie problemem jest częstotliwość przerwań z enkoderów, licz sprzętowo używając timerów.


PS
Dodaj obsługę WDG.
 
Odpowiedź
#5
Dziękuję bardzo za miłe słowa Smile

Wprowadziłem znów kilka zmian według wskazówek.
Częstotliwość występowania przerwań zewnętrznych zależy od wartości PWM podawanej na sterownik silników, a więc od prędkości obrotowej. Próbowałem na większych i mniejszych wartościach. Zauważyłem, że przy wartościach większych, występował jedynie błąd wynikający z pędu pojazdu. Silniki są hamowane natychmiastowo, jednak pęd zawsze pchnie trochę pojazd w stronę poruszania się. Przy mniejszych wartościach nic się nie zmieniało.
Miałem już wcześniej problem z przerwaniami zewnętrznymi (przerwania pojawiały się bez zmiany stanu na pinie 2), ale pomogło wykorzystanie innego pinu do przerwań zewnętrznych (zmiana D2 z D7). 
Aktualnie problem błędnego odtwarzania trasy udało mi się wyeliminować zmianą pinów przerwań zewnętrznych miejscami między enkoderami. 
Enkoder A był wpięty na pin 3, natomiast enkoder B na pin 7. Teraz wpięte są odwrotnie.
Pozostaje mi dojść do tego, z jakiego powodu podczas jazdy na wprost pojazd najpierw zaczyna kręcić prawym kołem, a po chwili (milisekundy) dopiero dołącza drugie koło. Podczas jazdy do tyłu problem ten nie występuje. 
Próbowałem z nastawami regulatora PD, ale jeśli to byłoby przyczyną, to problem występował by także podczas jazdy do tyłu. 
W funkcji stop() celowo zastosowałem delay(200), ponieważ wtedy silniki prawidłowo zatrzymują się natychmiastowo, a podczas wykonywania tej funkcji program nie musi wykonywać w tym czasie absolutnie nic innego. Stosuję uśpienie sterownika w celu oszczędzania energii, ale głównie z powodu późniejszego wystawienia odpowiednich stanów wyjść, w celu ustawienia jazdy w konkretnym kierunku i o odpowiedniej prędkości, po czym dopiero wybudzam sterownik. Dzięki temu silniki powinny startować dokładnie w tej samej chwili czasowej. Jednak podczas jazdy na wprost dzieje się inaczej i nie wiem co jest tego przyczyną.
Zastosowałem Watchdog'a, jednak proszę ocenić, czy zrobiłem to prawidłowo. Przyznaję się, że stosuję tę funkcję po raz pierwszy Smile Podczas odtwarzania trasy (pętla, której wcześniej program nie chciał opuszczać) za każdym razem kasuję licznik WDG, ponieważ może to być długa trasa w danym kierunku, co może trwać nawet ponad 10 sekund.
Poniżej zamieszczam aktualny kod programu:
Kod:
#include <avr/wdt.h>
#define A_ENC 7 //enkoder silnika A
#define B_ENC 3 //enkoder silnika B
#define STDBY 4 //uśpienie sterownika silników
#define ALR 5 //sygnał alarmu
#define A_IN1 2 //zmiana kierunku obrotów silnika prawego
#define A_IN2 6 //
#define A_PWM 9 //wyjście PWM na silnik prawy
#define B_IN1 8 //zmiana kierunku obrotów silnika prawego
#define B_IN2 11 //
#define B_PWM 10 //wyjście PWM na silnik lewy
volatile unsigned int A_enc = 0; // impulsy enkodera silnika A
volatile unsigned int B_enc = 0; // impulsy enkodera silnika B
byte line = 0; //numer wiersza macierzy (krok wykonania ruchu powrotnego)
unsigned int macierz [126][4] = {}; //tablica danych trasy przejazdu
byte Speed1 = 120; //prędkość obrotów silników w trybie normalnym
byte Speed2 = 60; //prędkość obrotów silników w trybie nauki
byte Speed3 = 60; //prędkość obrotów silników w trybie powrotu
const int w_zad = 0; //wartość zadana
const int k_p = 4; //współczynnik wzmocnienia członu P regulatora
const int k_d = 1; //stała różniczkowania regulatora
volatile int p; //wartość wyjściowa członu typu P
volatile int d; //wartość wyjściowa członu typu D
volatile int u; //aktualny uchyb regulacji
volatile int u_p = 0; //poprzedni uchyb regulacji
volatile int out; // sygnał wyjściowy regulatora
volatile int diff = 0; //różnica wskazań enkoderów
volatile int x; // regulowana wartość PWM A
volatile int y; // regulowana wartość PWM B
boolean Forward = 0; //znacznik jazdy do przodu
boolean Backward = 0; //znacznik jazdy do tyłu
boolean Left = 0; //znacznik jazdy w lewo
boolean Right = 0; //znacznik jazdy w prawo
char state = 0; //odczyt z bluetooth(SerialPort1)
int meas_sum = 0; // suma wartości odczytanych z ADC
byte meas_count = 0; //licznik próbek pomiaru napięcia 
int meas_av = 0; // uśredniona wartość napięcia
float volt; // obliczona wartość napięcia

void setup() {
  
  wdt_enable(WDTO_1S);
  pinMode(A_ENC, INPUT_PULLUP); //kofiguracja wejść i wyjść
  pinMode(B_ENC, INPUT_PULLUP);
  pinMode(ALR, OUTPUT);
  pinMode(STDBY, OUTPUT);
  pinMode(A_IN1, OUTPUT);
  pinMode(A_IN2, OUTPUT);
  pinMode(A_PWM, OUTPUT);
  pinMode(B_IN1, OUTPUT);
  pinMode(B_IN2, OUTPUT);
  pinMode(B_PWM, OUTPUT);
  Serial1.begin(9600); //włączenie transmisji szeregowej i ustawienie prędkości
  Serial.begin(9600);
  attachInterrupt(digitalPinToInterrupt(A_ENC), COUNT_A, CHANGE); // włączenie przerwań zewnętrznych
  attachInterrupt(digitalPinToInterrupt(B_ENC), COUNT_B, CHANGE);
  wdt_reset();
}
void loop() {

    if(meas_count == 10){
       meas_av = meas_sum/11;
       meas_count = 0;
       meas_sum = 0;
    }
    meas_sum += analogRead(A0);   // pomiar napięcia baterii 
    meas_count ++;
  
  volt = meas_av * (0.0097752);  // przeliczenie na wartość dziesiętną
  Serial1.print(" "); // wysłanie wartości przez bluetooth
  Serial1.print(volt); //
  
  /*while (volt < 5.0){ //kontrola napięcia
    digitalWrite(ALR, !digitalRead(ALR)); // napięcie poniżej 5V włącza alarm
    Stop();
    delay(500);
    wdt_reset();
  }*/
  digitalWrite(ALR, LOW); // wyłączenie alarmu
  
  if (state == '0') { // czyszczenie odczytów z enkoderów
    clearenc();
  } 
  if (Serial1.available()) { // odczyt danych z bluetooth
    state = Serial1.read(); // przypisanie odebranej danej do zmiennej
  }
  switch (state) {
    case '0': //stop
      Stop();
      clearenc();
      break;
    case '1': //
      forward(Speed1); //do przodu
      break;
    case '2':
      backward(Speed1); //do tyłu
      break;
    case '3':
      left(Speed1); //w lewo
      break;
    case '4':
      right(Speed1); //w prawo
      break;
    case '5': // nauka
      clearenc();
      break;
    case '6':                          // powrót
      for (char  i = 125; i >= 0; i--) {
        clearenc();
        if (macierz[i][0] > 0) {
          while (A_enc < macierz[i][0]) { //do tyłu
            backward(Speed3);
            wdt_reset();
          }
          Stop();
          macierz[i][0] = 0;
        }
        else if (macierz[i][1] > 0) { // do przodu
          while (A_enc < macierz[i][1]) {
            forward(Speed3);
            wdt_reset();
          }
          Stop();
          macierz[i][1] = 0;
        }
        else if (macierz[i][2] > 0) { // w prawo
          while (A_enc < macierz[i][2]) {
            right(Speed3);
            wdt_reset();
          }
          Stop();
          macierz[i][2] = 0;
        }
        else if (macierz[i][3] > 0) { // w lewo
          while (A_enc < macierz[i][3]) {
            left(Speed3);
            wdt_reset();
          }
          Stop();
          macierz[i][3] = 0;
        }
      }
      line = 0;
      break;
    case '7': //do przodu w trybie nauki
      forward(Speed2);
      Forward = 1;
      break;
    case '8': //do tyłu w trybie nauki
      backward(Speed2);
      Backward = 1;
      break;
    case '9': //w lewo w trybie nauki
      left(Speed2);
      Left = 1;
      break;
    case 'A': //w prawo w trybie nauki
      right(Speed2);
      Right = 1;
      break;
    case 'B':
      Stop(); // zatrzymanie w trybie nauki + zapamiętanie przejechanej trasy
      if (Forward == 1) { //zapisanie drogi do przodu
        macierz [line] [0] = A_enc;
      }
      else if (Backward == 1) { //zapisanie drogi do tyłu
        macierz [line] [1] = A_enc;
      }
      else if (Left == 1) { //zapisanie obrotu w lewo
        macierz [line] [2] = A_enc;
      }
      else if (Right == 1) { //zapisanie obrotu w prawo
        macierz [line] [3] = A_enc;
      }
      line ++;
      Forward = 0;
      Backward = 0;
      Left = 0;
      Right = 0;
      clearenc();
      break;
  }
  wdt_reset();
}
void COUNT_A() { //zliczanie impulsów ennkodera silnika A
  A_enc++;  
}  
void COUNT_B() { //zliczanie impulsów ennkodera silnika B
  B_enc++;
}
void forward(byte Speed) { //funkcja jazdy do przodu + synchronizacja prędkości obrotowej silników
  diff = A_enc - B_enc;
  u = w_zad - diff;
  p = k_p * u;
  d = k_d * (u_p - u);
  out = p + d;
  u_p = u;
  x = Speed - out;
  y = Speed + out;
  if(x > 255){
    x = 255;
  }else if(x < 35){
    x = 0;
  }
  if(y > 255){
    y = 255;
  }else if(y < 35){
    y = 0;
  }
  digitalWrite(A_IN1, HIGH); 
  digitalWrite(A_IN2, LOW);
  digitalWrite(B_IN1, LOW); 
  digitalWrite(B_IN2, HIGH);
  analogWrite(B_PWM, x);
  analogWrite(A_PWM, y);
  digitalWrite(STDBY, HIGH);
}
void backward(byte Speed) { //funkcja jazdy do tyłu + synchronizacja prędkości obrotowej silników
  diff = A_enc - B_enc;
  u = w_zad - diff;
  p = k_p * u;
  d = k_d * (u_p - u);
  out = p + d;
  u_p = u;
  x = Speed - out;
  y = Speed + out;
  if(x > 255){
    x = 255;
  }else if(x < 35){
    x = 0;
  }
  if(y > 255){
    y = 255;
  }else if(y < 35){
    y = 0;
  }
  digitalWrite(A_IN1, LOW); 
  digitalWrite(A_IN2, HIGH);
  digitalWrite(B_IN1, HIGH); 
  digitalWrite(B_IN2, LOW);
  analogWrite(B_PWM, x);
  analogWrite(A_PWM, y);
  digitalWrite(STDBY, HIGH);
}
void left(byte Speed) { //funkcja jazdy w lewo  + synchronizacja prędkości obrotowej silników
  diff = A_enc - B_enc;
  u = w_zad - diff;
  p = k_p * u;
  d = k_d * (u_p - u);
  out = p + d;
  u_p = u;
  x = Speed - out;
  y = Speed + out;
  if(x > 255){
    x = 255;
  }else if(x < 35){
    x = 0;
  }
  if(y > 255){
    y = 255;
  }else if(y < 35){
    y = 0;
  }
  digitalWrite(A_IN1, HIGH); 
  digitalWrite(A_IN2, LOW);
  digitalWrite(B_IN1, HIGH); 
  digitalWrite(B_IN2, LOW);
  analogWrite(B_PWM, x);
  analogWrite(A_PWM, y);
  digitalWrite(STDBY, HIGH);
  
}
void right(byte Speed) { //funkcja jazdy w prawo  + synchronizacja prędkości obrotowej silników
  diff = A_enc - B_enc;
  u = w_zad - diff;
  p = k_p * u;
  d = k_d * (u_p - u);
  out = p + d;
  u_p = u;
  x = Speed - out;
  y = Speed + out;
  if(x > 255){
    x = 255;
  }else if(x < 35){
    x = 0;
  }
  if(y > 255){
    y = 255;
  }else if(y < 35){
    y = 0;
  }
  digitalWrite(A_IN1, LOW); 
  digitalWrite(A_IN2, HIGH);
  digitalWrite(B_IN1, LOW); 
  digitalWrite(B_IN2, HIGH);
  analogWrite(B_PWM, x);
  analogWrite(A_PWM, y);
  digitalWrite(STDBY, HIGH);
}
void Stop() {
  digitalWrite(A_IN1, HIGH); // stop
  digitalWrite(A_IN2, HIGH);
  digitalWrite(B_IN1, HIGH); 
  digitalWrite(B_IN2, HIGH);
  analogWrite(A_PWM, 0);
  analogWrite(B_PWM, 0);
  delay(200);
  digitalWrite(STDBY, LOW);
}
void clearenc() { //funkcja czyszczenia odczytów z enkoderów
  A_enc = 0;
  B_enc = 0;
}
 
Odpowiedź
#6
(06-01-2019, 22:07)BourbonKid napisał(a): Dziękuję bardzo za miłe słowa Smile

Wprowadziłem znów kilka zmian według wskazówek.
Częstotliwość występowania przerwań zewnętrznych zależy od wartości PWM podawanej na sterownik silników, a więc od prędkości obrotowej. Próbowałem na większych i mniejszych wartościach. Zauważyłem, że przy wartościach większych, występował jedynie błąd wynikający z pędu pojazdu. (...)
Pozostaje mi dojść do tego, z jakiego powodu podczas jazdy na wprost pojazd najpierw zaczyna kręcić prawym kołem, a po chwili (milisekundy) dopiero dołącza drugie koło. Podczas jazdy do tyłu problem ten nie występuje. 
Próbowałem z nastawami regulatora PD, ale jeśli to byłoby przyczyną, to problem występował by także podczas jazdy do tyłu. 

(...)

Zastosowałem Watchdog'a, jednak proszę ocenić, czy zrobiłem to prawidłowo. Przyznaję się, że stosuję tę funkcję po raz pierwszy Smile Podczas odtwarzania trasy (pętla, której wcześniej program nie chciał opuszczać) za każdym razem kasuję licznik WDG, ponieważ może to być długa trasa w danym kierunku, co może trwać nawet ponad 10 sekund.
Co do problemów z enkoderem, podłącz oscyloskop lub ostatecznie analizator logiczny, miernik czestotliwości i zobacz jakie to są czestotliwości. Używając GPIO (ustawiając w przerwaniu) i oscyloskopu, możesz w przybliżeniu zmierzyć czas przerwań, zobaczyć zależności pomiędzy sygnałami.


Zanim o WDT. Zmień "volt" na "int" lub "long" (zależy jaką chcesz uzyskać rozdzielczość" i odpowiednio przeskaluj. Nawet na ARM unikam liczb zmiennoprzecinkowych. Pierwiastków tu nie liczysz, zakres duży nie jest, więc float jest zbędny.

Nadużywanie liczb zmiennoprzecinkowych to drugi po delay najczęstszy błąd Arduinowców!

Co do WDT. Ideałem jest, gdy w jednym miejscu pętli głównej WDT jest resetowany. Wymaga to pisania funkcji nieblokujących. Zysk jest oczywisty, WDT można resetować np 10 czy 100razy na sekundę i tak, można ustawić czas zadziałania WDG (w AVR nie jest zbyt elastyczny ten czas, więc np 125ms dla 10razy czy 16ms dla 100razy).
Nawet WDG w ARM jest zdecydowanie lepszy niż w AVR.
Można oczywiście resetowanie WDT umieszczać w długo wykonujących się funkcjach, ale takich, w dobrym programie, nie powinno być. Bo nicy co ma się długo wykonywać w takim robocie? Jazda prosto 10minut? Zlecam jazdę prosto przez 10 minut i wychodzę w funkcji. Przerwania po 10 minutach zatrzymają robota. Proste?
 
Odpowiedź
  


Skocz do:


Przeglądający: 2 gości