Skocz do zawartości

[Bascom] Line Follower algorytm


aixI

Pomocna odpowiedź

Procedurami byłoby fajnie, tak jak mówisz na pewno porządek byłby większy. Czytałem o procedurach w Bascom'ie tyko za bardzo nie wiem jak miałoby to wyglądać.

Coś w tym stylu?

'Sterowanie prawym silnikiem 
Sub P_silnik(byval Szybkosc As Integer) 
  If Szybkosc > 0 Then 
     H_in1 = 1 
     H_in2 = 0 
 else 
     H_in1 = 0 
     H_in2 = 1 
  End If 

  Pwm1b = Abs(szybkosc) 
 End Sub 

'##################################################################### 
'Sterowanie lewym silnikiem 
Sub L_silnik(byval Szybkosc As Integer) 
  If Szybkosc > 0 Then 
     H_in3 = 1 
     H_in4 = 0 
 else 
     H_in3 = 0 
     H_in4 = 1 
  End If 

  Pwm1a = Abs(szybkosc) 
End Sub 

albo

Sub P_silnik(byval Szybkosc As Integer) 
  If Szybkosc > 0 Then 
  In1 = 1 
  In2 = 0 
  Pwm1a = Szybkosc 
  End If 

  If Szybkosc < 0 Then 
  In1 = 0 
  In2 = 1 
  Szybkosc = Abs(szybkosc) 
  Pwm1a = Szybkosc 
  End If 

  If Szybkosc = 0 Then 
  In1 = 0 
  In2 = 0 
  Pwm1a = 0 
  End If 
End Sub
Link do komentarza
Share on other sites

Poprawiłem nieco kod, dodałem deklaracje itd. Program się kompiluje. Na razie nie testowany.

'Program by aixi 
'uC: ATmega16 (DIL_40)

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


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


'Mostek
'lewy'
In_1_a Alias Portd.2
In_1_b Alias Portd.6

'prawy'
In_2_a Alias Portd.7
In_2_b Alias Portd.3


Config Pina.0 = Input
Config Pina.1 = Input
Config Pina.2 = Input
Config Pina.3 = Input
Config Pina.4 = Input
Config Pina.5 = Input

Czujnik_1 Alias Pina.0
Czujnik_2 Alias Pina.1
Czujnik_3 Alias Pina.2
Czujnik_4 Alias Pina.3
Czujnik_5 Alias Pina.4
Czujnik_6 Alias Pina.5


Config Portb = Output
Config Portd = Output

Led_blue Alias Portb.0
Led_red Alias Portb.1


Declare Sub P_silnik(byval Szybkosc As Integer)
Declare Sub L_silnik(byval Szybkosc As Integer)
Dim V_l As Integer , V_p As Integer , V_nowa As Integer
Dim V_doc As Integer , V_min As Integer , V_max As Integer

Dim X As Byte
Dim Black As Byte
Dim Suma As Integer

Dim Uchyb_regulacji As Integer
Dim Zmiana As Single
Dim Zmiana_integer As Integer
Dim Kp As Integer
Dim P As Single
Dim Aktualna_pozycja As Single

Dim Czujniki(6) As Byte

V_doc = 175
V_max = 255
V_min = 165

Const Pozycja_docelowa = 0
Kp = 1

Call P_silnik(v_doc)
Call L_silnik(v_doc)

Set Led_red

Do
     Gosub Odczytaj_czujniki
     Gosub Licz_pozycje
     Gosub Licz_p

     Call P_silnik(v_p)
     Call L_silnik(v_l)
Loop
End

'==============================================================================

Licz_p:

Aktualna_pozycja = Suma / Black
Uchyb_regulacji = Pozycja_docelowa - Aktualna_pozycja
P = Uchyb_regulacji * Kp

P = P / 10
Zmiana = P
Zmiana_integer = Zmiana



'większa'
If Zmiana_integer > 0 Then
  V_nowa = V_doc + Zmiana_integer
     If V_nowa <= V_max Then
        V_p = V_nowa
     Else
        V_p = V_max
        V_l = V_doc - Zmiana_integer
        If V_l < V_min Then V_l = V_min
     End If
End If

'mniejsza'
If Zmiana_integer < 0 Then
  V_nowa = V_doc - Zmiana_integer
     If V_nowa <= V_max Then
        V_l = V_nowa
     Else
        V_l = V_max
        V_p = V_doc + Zmiana_integer
        If V_p < V_min Then V_p = V_min
     End If
End If

If Zmiana = 0 Then
  V_l = V_doc
  V_p = V_doc
End If

Return

'==============================================================================

Odczytaj_czujniki:

If Czujnik_1 = 1 Then                                       '1 czujnik
    Czujniki(1) = 1
Else
    Czujniki(1) = 0
End If

If Czujnik_2 = 1 Then                                       '2 czujnik
    Czujniki(2) = 1
Else
    Czujniki(2) = 0
End If

If Czujnik_3 = 1 Then                                       '3 czujnik
    Czujniki(3) = 1
Else
    Czujniki(3) = 0
End If
'==============================================
If Czujnik_4 = 1 Then                                       '4 czujnik
    Czujniki(4) = 1
Else
    Czujniki(4) = 0
End If

If Czujnik_5 = 1 Then                                       '5 czujnik
    Czujniki(5) = 1
Else
    Czujniki(5) = 0
End If

If Czujnik_6 = 1 Then                                       '6 czujnik
    Czujniki(6) = 1
Else
    Czujniki(6) = 0
End If

Return

'==============================================================================

Licz_pozycje:

X = 1
  Black = 0
  Suma = 0

While X <= 6
 If Czujniki(x) = 1 Then
   Black = Black + 1
   Select Case X
   Case 1:
     Suma = Suma - 3
   Case 2:
     Suma = Suma - 2
   Case 3:
     Suma = Suma - 1
   Case 4:
     Suma = Suma + 1
   Case 5:
     Suma = Suma + 2
   Case 6:
     Suma = Suma + 3
   End Select
 End If
X = X + 1
Wend

X = 0

Return

'==============================================================================

'Sterowanie prawym silnikiem'
Sub P_silnik(byval Szybkosc As Integer)

  If Szybkosc > 0 Then
     In_2_a = 1
     In_2_b = 0
        Pwm1a = Szybkosc
  End If

  If Szybkosc < 0 Then
     In_2_a = 0
     In_2_b = 1
        Szybkosc = Abs(szybkosc)
        Pwm1a = Szybkosc
  End If

  If Szybkosc = 0 Then
     In_2_a = 0
     In_2_b = 0
        Pwm1a = 0
  End If
End Sub

'==================================================

'Sterowanie lewym silnikiem'
Sub L_silnik(byval Szybkosc As Integer)

  If Szybkosc > 0 Then
     In_1_a = 1
     In_1_b = 0
        Pwm1b = Szybkosc
  End If

  If Szybkosc < 0 Then
     In_1_a = 0
     In_1_b = 1
        Szybkosc = Abs(szybkosc)
        Pwm1b = Szybkosc
  End If

  If Szybkosc = 0 Then
     In_1_a = 0
     In_1_b = 0
        Pwm1b = 0
  End If
End Sub
Link do komentarza
Share on other sites

Nie zapomniałeś wywoływać funkcje z sterowaniem silników przypadkiem?

Oj szczerze mówiąc, to coraz słabiej widzę tego LFa...

Link do komentarza
Share on other sites

Zarejestruj się lub zaloguj, aby ukryć tę reklamę.
Zarejestruj się lub zaloguj, aby ukryć tę reklamę.

jlcpcb.jpg

jlcpcb.jpg

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

Powiem tak:

Jest jakiś progress, ponieważ robot jeździ i jakoś "chce" trzymać się linii, tylko ma problem ze skrętami, tzn. jedzie, jedzie, (i jest skręt) nieznacznie skręci w stronę skrętu/linii i dalej jedzie prosto. Zmieniałem wagi, wzmocnienie Kp ale za bardzo to nie pomaga.

Parę postów wyżej piszesz, że wygląda to (tzn. procedury odpowiedzialne za kierowanie silnikami) dobrze i, że muszę przetestować, więc testuje - zmieniam kod i w ogóle, a potem piszesz, że coraz słabiej widzisz tego lf'a. To jednak nie pomaga :/

Ja chce się czegoś nauczyć, nikt za mnie tego nie zrobi. I tak dziękuję Ci za dotychczasową pomoc, bo na prawdę się przydaje (pewne doświadczenie w tym co się robi). Osobiście myślę, że projekt zmierza w dobrą stronę. Wiem też, że niektóre rzeczy od Was nie zależą tylko ode mnie, człoweik uczy się na błędach.

'Program by aixi
'uC: ATmega16 (DIL_40)

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


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


'Mostek
'lewy'
In_1_a Alias Portd.2
In_1_b Alias Portd.6

'prawy'
In_2_a Alias Portd.7
In_2_b Alias Portd.3


Config Pina.0 = Input
Config Pina.1 = Input
Config Pina.2 = Input
Config Pina.3 = Input
Config Pina.4 = Input
Config Pina.5 = Input

Czujnik_1 Alias Pina.0
Czujnik_2 Alias Pina.1
Czujnik_3 Alias Pina.2
Czujnik_4 Alias Pina.3
Czujnik_5 Alias Pina.4
Czujnik_6 Alias Pina.5


Config Portb = Output
Config Portd = Output

Led_blue Alias Portb.0
Led_red Alias Portb.1


Declare Sub P_silnik(byval Szybkosc As Integer)
Declare Sub L_silnik(byval Szybkosc As Integer)
Dim V_l As Integer , V_p As Integer , V_nowa As Integer
Dim V_doc As Integer , V_min As Integer , V_max As Integer

Dim X As Byte
Dim Black As Byte
Dim Suma As Integer

Dim Uchyb_regulacji As Integer
Dim Zmiana As Single
Dim Zmiana_integer As Integer
Dim Kp As Integer
Dim P As Single
Dim Aktualna_pozycja As Single

Dim Czujniki(6) As Byte

V_doc = 175
V_max = 255
V_min = 165

Const Pozycja_docelowa = 0
Kp = 10

Call P_silnik(v_doc)
Call L_silnik(v_doc)

Set Led_red

Do
     Gosub Odczytaj_czujniki
     Gosub Licz_pozycje
     Gosub Licz_p

     Call P_silnik(v_p)
     Call L_silnik(v_l)

'którykolwiek czujnik na linii - LED się zapali(działa)
If Black > 0 Then
  Led_blue = 1
Else
  Led_blue = 0
End If

Loop
End

'==============================================================================

Licz_p:

Aktualna_pozycja = Suma / Black
Uchyb_regulacji = Pozycja_docelowa - Aktualna_pozycja
P = Uchyb_regulacji * Kp

'P = P / 10
Zmiana = P
Zmiana_integer = Zmiana



'większa'
If Zmiana_integer > 0 Then
  V_nowa = V_doc + Zmiana_integer
     If V_nowa <= V_max Then
        V_p = V_nowa
     Else
        V_p = V_max
        V_l = V_doc - Zmiana_integer
        If V_l < V_min Then V_l = V_min
     End If
End If

'mniejsza'
If Zmiana_integer < 0 Then
  V_nowa = V_doc - Zmiana_integer
     If V_nowa <= V_max Then
        V_l = V_nowa
     Else
        V_l = V_max
        V_p = V_doc + Zmiana_integer
        If V_p < V_min Then V_p = V_min
     End If
End If

If Zmiana_integer = 0 Then
  V_l = V_doc
  V_p = V_doc
End If

Return

'==============================================================================

Odczytaj_czujniki:

If Czujnik_1 = 1 Then                                       '1 czujnik
    Czujniki(1) = 1
Else
    Czujniki(1) = 0
End If

If Czujnik_2 = 1 Then                                       '2 czujnik
    Czujniki(2) = 1
Else
    Czujniki(2) = 0
End If

If Czujnik_3 = 1 Then                                       '3 czujnik
    Czujniki(3) = 1
Else
    Czujniki(3) = 0
End If
'==============================================
If Czujnik_4 = 1 Then                                       '4 czujnik
    Czujniki(4) = 1
Else
    Czujniki(4) = 0
End If

If Czujnik_5 = 1 Then                                       '5 czujnik
    Czujniki(5) = 1
Else
    Czujniki(5) = 0
End If

If Czujnik_6 = 1 Then                                       '6 czujnik
    Czujniki(6) = 1
Else
    Czujniki(6) = 0
End If

Return

'==============================================================================

Licz_pozycje:

X = 1
  Black = 0
  Suma = 0

While X <= 6
 If Czujniki(x) = 1 Then
   Black = Black + 1
   Select Case X
   Case 1:
     Suma = Suma - 30
   Case 2:
     Suma = Suma - 20
   Case 3:
     Suma = Suma - 10
   Case 4:
     Suma = Suma + 10
   Case 5:
     Suma = Suma + 20
   Case 6:
     Suma = Suma + 30
   End Select
 End If
X = X + 1
Wend

X = 0

Return

'==============================================================================

'Sterowanie prawym silnikiem'
Sub P_silnik(byval Szybkosc As Integer)

  If Szybkosc > 0 Then
     In_2_a = 0
     In_2_b = 1
        Pwm1a = Szybkosc
  End If

  If Szybkosc < 0 Then
     In_2_a = 1
     In_2_b = 0
        Szybkosc = Abs(szybkosc)
        Pwm1a = Szybkosc
  End If

  If Szybkosc = 0 Then
     In_2_a = 0
     In_2_b = 0
        Pwm1a = 0
  End If
End Sub

'==================================================

'Sterowanie lewym silnikiem'
Sub L_silnik(byval Szybkosc As Integer)

  If Szybkosc > 0 Then
     In_1_a = 0
     In_1_b = 1
        Pwm1b = Szybkosc
  End If

  If Szybkosc < 0 Then
     In_1_a = 1
     In_1_b = 0
        Szybkosc = Abs(szybkosc)
        Pwm1b = Szybkosc
  End If

  If Szybkosc = 0 Then
     In_1_a = 0
     In_1_b = 0
        Pwm1b = 0
  End If
End Sub
Link do komentarza
Share on other sites

Parę postów wyżej piszesz, że wygląda to (tzn. procedury odpowiedzialne za kierowanie silnikami) dobrze i, że muszę przetestować, więc testuje - zmieniam kod i w ogóle, a potem piszesz, że coraz słabiej widzisz tego lf'a. To jednak nie pomaga :/

No tak, bo testy testami, ale efektów było brak, ale teraz są 😉

Jak za słabo skręca to zwiększaj Kp tak długo aż będzie dostawał "drgawek" na linii.

Link do komentarza
Share on other sites

Spróbuję pobawić się Kp 😃 A do testów jakie wartości wag ustawić? (dla 6 czujników)

A Twoim okiem jak wygląda teraz obecny algorytm?

Link do komentarza
Share on other sites

Jak na początek wagi są ok, później się tym zajmiesz. Teraz pobaw się tym algorytmem, tak abyś "poczuł" jak to wszystko działa.

Link do komentarza
Share on other sites

Teraz pobaw się tym algorytmem, tak abyś "poczuł" jak to wszystko działa.

Prawda. Bawię się ustawieniami Kp, nawet prędkości silników (V_doc, V_min, V_max) i na razie bez skutku. Próbuję zrozumieć te procedury do sterowania silnikami (P_silnik, L_silnik). Skąd program wie kiedy "Szybkosc" jest >, < bądź = 0? (wiem, że to pytania hmm... laika w dziedzinie programowania ale to pierwsze, głębsze zetknięcie z nimi).

Link do komentarza
Share on other sites

No jak skąd wie, przecież w procedurach masz ify, które to rozpoznają. Sprecyzuj swoje pytanie.

Liczenie P masz zagmatwane, tam powinno być (pisząc pseudokodem):

lewy = docelowa + zmiana

prawy = docelowa - zmiana

Jeśli lewy max, to lewy = max. Analogiczni dla prawego.

Nie trzeba rozbijać tego na zmiana większa/mniejsza od 0. Znaki same to załatwią. Skup się popracuj trochę i zrób sporo eksperymentów. Inaczej moje pisanie nic nie da.

Link do komentarza
Share on other sites

Pobawiłem się trochę tym liczeniem P, poeksperymentowałem, ogarnąłem trochę kod, tzn. zrozumiałem niektóre rzeczy. Teraz robot umie pokonać zakręt 🙂. Resztę eksperymentów i testów będę robił jak już polutuję płytkę główną i płytkę z czujnikami, bo testy na podwoziu testowym (z wiszącymi kabelkami i płytami stykowymi... sami wiecie 😃 ). Pracę będę kończył w soboty no i w wakacje (jeżeli nie skończę wcześniej). Jak na razie dziękuje Wszystkim za zainteresowanie tematem no i Tobie Treker za baaaaardzo dużą pomoc 🙂

Link do komentarza
Share on other sites

To pokaż jakiś filmik chociaż na zakończenie 😉 Wiadomo w końcu gdzie był błąd? Miałeś pomieszane procedury od sterowania obrotami?

Link do komentarza
Share on other sites

To pokaż jakiś filmik chociaż na zakończenie 😉

Jak ukończę projekt, to pewnie opublikuję go tu na Forbocie.

Wiadomo w końcu gdzie był błąd? Miałeś pomieszane procedury od sterowania obrotami?

Pobawiłem się procedurami i je rozgryzłem, wiem teraz jak działa program (z czego jestem zadowolony 🙂), a błąd? Błąd leżał pewnie po stronie wysterowania silników. Choć ten kod lepiej przemawia do mnie i jest na pewno lepiej zorganizowany - czytelniejszy.

  • Lubię! 1
Link do komentarza
Share on other sites

Tak, tak, jeszcze brakuje członu różniczkującego, ale wcześniej sobie rozpisałem jak ma to mniej więcej wyglądać - mam nadzieje, że dobrze 🙂

Dim Uchyb_regulacji As Integer
Dim Aktualna_pozycja As Single
Const Pozycja_docelowa = 0

Dim Rozniczka As Integer
Dim Pop_uchyb_reg As Integer
Dim D As Single
Dim Kd As Integer



Aktualna_pozycja = Suma / Black 

Uchyb_regulacji = Pozycja_docelowa - Aktualna_pozycja

Rozniczka = Uchyb_regulacji - Pop_uchyb_reg

Pop_uchyb_reg = Uchyb_regulacji


P = Uchyb_regulacji * Kp
D = Rozniczka * Kd 


Zmiana = P + D

Zmiana_integer = Zmiana

Pracuje też na jakimś debugerem na PC'a w Delphi. Mam już zarys aplikacji do tego.

Wygląda tak:

Jedynie co działa to przycisk "Exit" 😃 Jeszcze do aplikacji chcę dodać 16 szt. czegoś co będzie się (no nie wiem) zapalało, gdy odpowiedni czujnik będzie na linii. Czyli, np. czujnik5 na linii, zapala się "coś5", które sygnalizuje, że ten czujnik jest na linii.

Link do komentarza
Share on other sites

Dołącz do dyskusji, napisz odpowiedź!

Jeśli masz już konto to zaloguj się teraz, aby opublikować wiadomość jako Ty. Możesz też napisać teraz i zarejestrować się później.
Uwaga: wgrywanie zdjęć i załączników dostępne jest po zalogowaniu!

Anonim
Dołącz do dyskusji! Kliknij i zacznij pisać...

×   Wklejony jako tekst z formatowaniem.   Przywróć formatowanie

  Dozwolonych jest tylko 75 emoji.

×   Twój link będzie automatycznie osadzony.   Wyświetlać jako link

×   Twoja poprzednia zawartość została przywrócona.   Wyczyść edytor

×   Nie możesz wkleić zdjęć bezpośrednio. Prześlij lub wstaw obrazy z adresu URL.

×
×
  • Utwórz nowe...

Ważne informacje

Ta strona używa ciasteczek (cookies), dzięki którym może działać lepiej. Więcej na ten temat znajdziesz w Polityce Prywatności.