[johnyNapalm]

Tworze autonomicznego robota składającego się z:
- dwóch czujników ultradźwiękowych (HC-SRo4)
- dwóch silników DC
- modułu kart mikro SD

Robot "śledzi" ściany oraz w zależności od czynności jakie wykonuje (jazda prosto, skręt w prawo, skręt w lewo) zrzuca na kartę SD cyfry 0, 1 lub 2.
Śledzenie ścian odbywa się za pomocą techniki sterownika PID.

Wzór na obliczanie prędkości silników: 

Kod:

Wall = sr04_left.Distance();
error_wall = Wall - offset;
i_wall = (i_wall + error_wall);
d_wall = (error_wall - lastError_wall);

correction_wall = Kp_wall * error_wall + Ki_wall * i_wall + Kd_wall * d_wall;

rightMotorSpeed  = 128 + correction_wall;
leftMotorSpeed = 128 - correction_wall;

Dodatkowo do robota tworzę aplikację rysującą mapy pomieszczeń na podstawie danych które robot zgrał na kartę SD.
Program rysuje mapy w następujący sposób:
- Jeżeli widzi 0 - rysuje linie do góry
- Jeżeli widzi 1 - zaczyna rysowanie kolejnych zer o -90*
- Jeżeli widzi 2 - zaczyna rysowanie kolejnych zer o 90*

Zdjęcie przykładowej trasy robota:
   

Problemem w moim projekcie jest niedokładność pomiarowa czujników. Przez co zrzucane są błędne informacje na kartę SD, a co za tym idzie mapa jest niepoprawnie rysowana przez program rysujący.
Pomyślałem więc że dobrym sposobem na zniwelowanie błędu pomiarowego będzie stworzenie tablicy np. 5 pomiarów odległości a następnie obliczenie z niej mediany, a dopiero z mediany obliczać prędkość silników.
Zaimplementowałem więc medianę: 

Kod:

int mediana(int n, int x[]) {
 int temp;
 int i, j;
 for (i = 0; i < n - 1; i++) {
   for (j = i + 1; j < n; j++) {
     if (x[j] < x[i]) {
       temp = x[i];
       x[i] = x[j];
       x[j] = temp;
     }
   }
 }
 return x[n / 2];
}

Oraz dodałem ją do wzoru PID:

Kod:

for (int i = 0; i < sizeof(distanceArray) - 1; i++) {
 distanceArray[i] =  sr04_left.Distance();
}
 
Wall = mediana(5, distanceArray);
error_wall = Wall - offset;
i_wall = (i_wall + error_wall);
d_wall = (error_wall - lastError_wall);

correction_wall = Kp_wall * error_wall + Ki_wall * i_wall + Kd_wall * d_wall;

[size=small][font=Monaco, Consolas, Courier, monospace]rightMotorSpeed  = 128 + correction_wall;[/font][/size]
leftMotorSpeed = 128 - correction_wall;

Niestety to rozwiązanie nie spełnia moich oczekiwań. Po dodaniu mediany do wzoru robot bardzo wolno "myśli". Zapewne nie rozwiązałem problemu poprawnie dlatego zwracam się z prośbą o pomoc. Mam nadzieję że wyczerpująco wytłumaczyłem problem, jeżeli nie to proszę o pytania.

Kod do progarmu:
https://pastebin.com/PqWz7HaZ

Zdjęcie robota: