[Bascom] Line Follower algorytm

[aixI]

Siemka wszystkim.

Słuchajcie mam taki problem, posiadam płytę z czujnikami KTIR'ami (jest ich 6szt.) i nie mogę sobie poradzić jak napisać algorytm (no nie wiem, chociażby sam algorytm typu P dla LF'a - żeby sobie to poukładać w głowie). Algorytm to raczej w Bascom'ie, uC - ATmega8 i inne.

Na 3 czujniki to łatwo napisać program, np. kod z kursu Nawyka (line follower dla początkujących)

Czytałem to:

* https://www.forbot.pl/forum/topics20/algorytmy-pid-w-robotyce-amatorskiej-linefollower-cz1-czlon-p-vt5973.htm?utm_source=forbot&utm_medium=link_spis_art&utm_content=art_5973&utm_campaign=spis_art

* https://www.forbot.pl/forum/topics20/algorytmy-sposoby-wykrywanie-linii-przez-roboty-linefollower-vt618.htm

* http://fahmizaleeits.wordpress.com/2010/05/08/robot-line-follower-dengan-kendali-pid/

* http://fahmizaleeits.wordpress.com/2010/08/24/tuning-kontrol-pid-pada-robot-line-follower/

https://www.forbot.pl/forum/topics50/algorytm-kod-do-lf-jezyk-c-vt7419.htm

Taki kawałem al'a kodu:

'to jako tako rozumiem, ale jak dalej napisać to czarna magia :/

Docelowa_pozycja = 0

Error = Docelowa_pozycja – Aktualna_pozycja

P = Kp * Error

Pwm1a = 127 + P

Pwm1b = 127 – P

Przypisywanie wag czujników:

Waga(1) = 13

Waga(2) = 12

Waga(3) = 11

Waga(4) = -11

Waga(5) = -12

Waga(6) = -13

No nie wiem, może ktoś będzie chętny mi jakoś pomóc, jakoś wytłumaczyć. Może ktoś z Was ma/widział jakąś książkę na temat algorytmów PID, PD, PI, P w Bascom'ie.

Jeżeli ktoś kto używał samego człona P w swoim LF'ie i byłby tak hojny do podzielenia się kawałkiem tego właśnie kodu.

Edit:

Napisałem taki kod i mam prośbę to Was, czy to co kombinuję idzie w dobrą drogę??

Do tego kodu posłużyłem się trochę na artykule Trekera, podlinkowanym powyżej.

$regfile = "m8def.dat"
$crystal = 1000000

'Konfiguracja przetwornika analog-cyfra (ADC)
Config Adc = Single , Prescaler = 64 , Reference = Avcc

'Konfiguracja sterowania sprzętowym PWM
Config Timer1 = Pwm , Pwm = 8 , Prescale = 1 , Compare A Pwm = Clear Down , Compare B Pwm = Clear Down

Config Portb = Output

Silnik_lewy_a Alias Portb.4
Silnik_lewy_b Alias Portb.0
Silnik_prawy_a Alias Portb.5
Silnik_prawy_b Alias Portb.3


Dim Kp As Integer, Tp As Integer, Granica As Integer
Dim Aktualna_pozycja As Single
Dim Suma_punktow As Integer , Ile_wykrylo As Byte
Dim Czujniki(6) As Integer , Flagalinii As Byte , Punkty(7) As Integer
Dim Czujniki_licznik As Byte
Dim P As Integer , Error As Integer


'Pukty przypisane do czujników
Punkty(1) = 13
Punkty(2) = 12

Punkty(3) = 11
Punkty(4) = -11

Punkty(5) = -12
Punkty(6) = -13


'Granica dla czujników linii
Granica = 700

Const Docelowa_pozycja = 0

Tp = 101                                                    '/docelowa prędkość silników 40% pwm
Kp = 0                                                      '/wzmocnienie

Pwm1a = Tp                                                  '/docelowa prędkość
Pwm1b = Tp                                                  '/docelowa prędkość

Do
Gosub Odczytaj_czujniki
Gosub Licz_pozycje


Aktualna_pozycja = Suma_punktow / Ile_wykrylo

Error = Docelowa_pozycja + Aktualna_pozycja

P = Error * Kp

  If P > 0 Then

      Pwm1b = Tp - P                                       '/right MOTOR slow
      Pwm1a = Tp + P                                       '/left MOTOR full

  Elseif P < 0 Then

      Pwm1b = Tp + P                                       '/right MOTOR  full
      Pwm1a = Tp - P                                       '/left MOTOR slow


  Else

      Pwm1b = 250                                          '/right MOTOR full
      Pwm1a = 250                                          '/left MOTOR full
End If

If P > 102 Then
P = 102
Elseif P < -102 Then
P = -102
End If

Loop
End

'/-------------------------------------------------------------


Odczytaj_czujniki:
  'Odczytujemy wszystkie czujniki po kolei
  For Czujniki_licznik = 0 To 5
     Czujniki(czujniki_licznik + 1) = Getadc(czujniki_licznik)
     If Czujniki(czujniki_licznik + 1) > Granica Then Set Flagalinii.czujniki_licznik Else Reset Flagalinii.czujniki_licznik
  Next
Return

'/-------------------------------------------------------------


Licz_pozycje:

        'Zerujemy zmienne
        Suma_punktow = 0
        Ile_wykrylo = 0

        'Sumujemy punkty z czujników, które wykryły linie
        For Czujniki_licznik = 0 To 5
           If Flagalinii.czujniki_licznik = 1 Then
              Suma_punktow = Suma_punktow + Punkty(czujniki_licznik + 1)
              Ile_wykrylo = Ile_wykrylo + 1
           End If
        Next
Return

[aixI]

EDIT (28.01.2014):

Cześć, złożyłem LF'a i zacząłem pisać program i jest progress. Ten program to algorytm P dla LF'a, robot jeździ po czarnej linie, tylko jest jeden problem - podczas jazdy za bardzo oscyluje (lewo, prawo), więc zacząłem zmieniać Kp, przy mniejszych wartościach oscylacje są mniejsze, ale nie wchodzi gładko w zakręty (a i tak bot nie jest zbyt szybki), a przy większych wartościach Kp robot gładko wchodzi w zakręty, ale podczas jazdy po prostej są oscylacje i gdy ustawi się środkowymi czujnikami (4 i 5) to zaczyna stać i nie wiem w czym problem. Czym to może być spowodowane?

[Treker (Damian Szymański)]

Zanim ustawisz wypełnienie PWM sprawdź czy wyliczone wartości nie przepełniły zmiennych.

Objawy, które opisujesz najłatwiej zniwelować członem różniczkującym.

Pokaż jakiś filmik, będzie łatwiej doradzić

[aixI]

Proszę, filmik:

https://www.youtube.com/watch?v=7HxH4itbiK0

Zanim ustawisz wypełnienie PWM sprawdź czy wyliczone wartości nie przepełniły zmiennych.

Jak dobrze rozumiem to chodzi o takie coś:

If P > 128 Then
P = 128
Elseif P < -128 Then
P = -128
End If

Czy może to, że zmienną P mam typu Single.

[Treker (Damian Szymański)]

Wybacz, że nie zagłębiam się w Twój kod. Co do ograniczenia chodzi mi o coś takiego.

[...]

P - wartość członu proporcjonalego

uint16_t V_L_temp = 0;
uint16_t V_P_temp = 0;

V_L_temp = Tp + P;
V_P_temp = Tp - P;

If (V_L_temp > 255) {
V_L_temp = 255;
}
// i takie warunki ponawiamy dla +255 i -255, dla obu sygnałów
[...]

OCR1A = V_L_temp;
OCR1B = V_P_temp;

Co do filmiku:

Kp jest za duże, pobaw się również wagami czujników

Dasz radę wycisnąć sporo więcej na samym P.

[aixI]

Kp jest za duże, pobaw się również wagami czujników

Ok, to do testów Kp ustawić na 1 i zwiększać/zmniejszać wagi czujników?

Wybacz, że nie zagłębiam się w Twój kod. Co do ograniczenia chodzi mi o coś takiego.

Dobrze, tylko ja nie rozumiem C :/ Programuję w Bascom'ie.

[Treker (Damian Szymański)]

A czego tam nie rozumiesz ? Nic specyficznego z C tam nie ma.

uint_16_t to zmienna typu int (coś większego od byte)

Kp = 1... Hmm mam nadzieję, że nie bawisz się w ułamkowe wartości?

Przeskaluj sobie wszystko, aby otrzymać większe możliwości regulacji.

Ile miałeś ustawione Kp na filmiku?

[aixI]

Ile miałeś ustawione Kp na filmiku?

Na pierwszej połowie jest Kp = 8, a na drugiej połowie Kp = 2,5.

Kp = 1... Hmm mam nadzieję, że nie bawisz się w ułamkowe wartości?

Staram się unikać, a i tak do nakręcenia filmiku dałem 2,5.

[Slawus]

na drugiej połowie Kp = 2,5.

Zacznijmy od tego, że zmienną Kp zadeklarowałeś jako typ Integer, który to może przechowywać zmienne tylko całkowite (bez przecinka).

Mimo wszystko, nie powinno mieć to większego wpływu na algorytm.

[aixI]

Treker, czy o takie coś chodziło?

Dim V_L_temp As Integer
Dim V_P_temp As Integer

V_L_temp = 0
V_P_temp = 0

'Tp - prędkość docelwa'

V_L_temp = Tp + P
V_P_temp = Tp - P

'lewy
If V_L_temp > 255 Then
  V_L_temp = 255
Elseif V_L_temp < -255 Then
  V_L_temp = -255
End If

'prawy
If V_P_temp > 255 Then
  V_P_temp = 255
Elseif V_P_temp < -255 Then
  V_P_temp = -255
End If

Pwm1a = V_L_temp
Pwm1b = V_P_temp

Slawus Dzięki za info.

[Treker (Damian Szymański)]

aixI, tak - wygląda poprawnie

[aixI]

Ok, to teraz mam to zaimplementować do kodu - dać to do pętli Do i powinno działać ?

[Treker (Damian Szymański)]

No tak, będziesz miał zabezpieczony program przed przepełnieniem zmiennych

[aixI]

Dobrze, wrzuciłem to do pętli i wygląda to tak:

Dim V_L_temp As Integer
Dim V_P_temp As Integer
Dim Tp As Byte

V_L_temp = 0
V_P_temp = 0

Tp = 127

Do

  Gosub Odczytaj_czujniki

Roznica = Sensor_value_sum / Sensor_on_black
P = Roznica * Kp

V_L_temp = Tp + P
V_P_temp = Tp - P


If Sensor_on_black > 0 Then
 Pwm1a = V_L_temp                                           'Pwm1a => lewy
 Pwm1b = V_P_temp                                           'Pwm1b => prawy
End If


'lewy
If V_L_temp > 255 Then
  V_L_temp = 255
Elseif V_L_temp < -255 Then
  V_L_temp = -255
End If

'prawy
If V_P_temp > 255 Then
  V_P_temp = 255
Elseif V_P_temp < -255 Then
  V_P_temp = -255
End If


If P > 128 Then
  P = 128
Elseif P < -128 Then
  P = -128
End If


Loop
End

Edit:

Martwi mnie i denerwuje jeden problem...

Jak dwa środkowe czujniki 'widzą' czarną linie to bot stoi. I żeby ponownie odbił się trzeba go przestawić aby jeden ze śr. czujników był na białym podłożu. Mam 8 czujników i wagi tych śr. czujników to -11 i 11.

[Treker (Damian Szymański)]

Co to jest?

If P > 128 Then 
   :P = 128 
Elseif P < -128 Then 
   :P = -128 
End If

A tu czasami możesz dzielić przez zero:

Roznica = Sensor_value_sum / Sensor_on_black