[BourbonKid]

Witam. Jestem nowy na forum, więc dzień dobry

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;
}

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