[Bascom] Line Follower algorytm

[Treker (Damian Szymański)]

Jak sprawdzasz zakres (+/-) dla prawego silnika to zmieniaj piny od kierunku tylko dla niego. Resztę zostaw, analogicznie dla lewego.

Jak mam to zrobić?

Jeśli prędkość jest ujemna to wewnątrz if'a robisz np.:

Pwm1a = 0-V_l_temp, wtedy z ujemnego zrobi się dodatni.

Który dokładnie warunek? Ten? --> If V_l_temp

'lewy 
If V_l_temp > 255 Then 
  V_l_temp = 255 
Elseif V_l_temp < 0 Then 
  V_l_temp = 0
End If 

i drugi tak samo.

[aixI]

Dobrze, poprawiłem nieco kod według Twoich wskazówek. Teraźniejszy kawałek kodu:

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

V_l_temp = Tp + Zmiana                                      'lewy
V_p_temp = Tp - Zmiana                                      'prawy

'lewy
If V_l_temp > 255 Then
  V_l_temp = 255
     Set Silnik_lewy_a
     Reset Silnik_lewy_b
Elseif V_l_temp < -255 Then                                    'Elseif V_l_temp < 0 Then                       
  V_l_temp = -255
     Reset Silnik_lewy_a
     Set Silnik_lewy_b
        Pwm1a = 0 - V_l_temp
End If


'prawy
If V_p_temp > 255 Then
  V_p_temp = 255
     Set Silnik_prawy_a
     Reset Silnik_prawy_b
Elseif V_p_temp < -255 Then                                    'Elseif V_p_temp < 0 Then
  V_p_temp = -255
     Reset Silnik_prawy_a
     Set Silnik_prawy_b
        Pwm1b = 0 - V_p_temp
End If


If Black >= 0 Then
 Pwm1a = V_l_temp                                          'Pwm1a => lewy
 Pwm1b = V_p_temp                                          'Pwm1b => prawy
End If

Loop
End

Myślę, że dobrze zrozumiałem o co Ci chodzi - to są wyniki z testu.

Gdy V_l_temp < 0, to V_l_temp = 0, to przy tych ustawieniach silniki piszczą i dobrze słychać zmianę wypełnienia, a jak są środkowe czujniki na linii to cisza - nic nie piszczy. Myślę, że dobrze zrozumiałem o co Ci chodzi.

Po tych zabiegach z PWM'em (tzn. Pwm1a = 0 - V_l_temp i dla Pwma1b) nie muszę już nic zmieniać w tym?

If Black >= 0 Then 
 Pwm1a = V_l_temp                                          'Pwm1a => lewy 
 Pwm1b = V_p_temp                                          'Pwm1b => prawy 
End If

[Treker (Damian Szymański)]

Zamiast:

If Black >= 0 Then 
 Pwm1a = V_l_temp                                          'Pwm1a => lewy 
 Pwm1b = V_p_temp                                          'Pwm1b => prawy 
End If 

Daj:

Pwm1a = V_l_temp                                          'Pwm1a => lewy 
Pwm1b = V_p_temp                                          'Pwm1b => prawy 

Testując ustaw Tp na 200, będzie łatwiej widać efekty.

[aixI]

Bez If'a ?

[Polecacz 101]

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

[Treker (Damian Szymański)]

Czekaj,,, to nadal jest źle. Powinno być:

If V_l_temp > 255 Then 
  V_l_temp = 255 
Elseif V_l_temp < -255 Then                                                  
  V_l_temp = -255 
End If

If V_l_temp >= 0 Then 
  Set Silnik_lewy_a 
  Reset Silnik_lewy_b
  Pwm1a = V_l_temp 
Else                                                
  Reset Silnik_lewy_a 
  Set Silnik_lewy_b 
  Pwm1a = 0 - V_l_temp 
End If 

Tak bez if'a, on i tak nic nie daje.

[aixI]

Poprawiony kawałek kodu:

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


V_l_temp = Tp + Zmiana                                      'lewy
V_p_temp = Tp - Zmiana                                      'prawy


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

If V_l_temp >= 0 Then
  Reset Silnik_lewy_a
  Set Silnik_lewy_b
     Pwm1a = V_l_temp
Else
  Set Silnik_lewy_a
  Reset Silnik_lewy_b
     Pwm1a = 0 - V_l_temp
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 V_p_temp >= 0 Then
  Reset Silnik_prawy_a
  Set Silnik_prawy_b
     Pwm1b = V_p_temp
Else
  Set Silnik_prawy_a
  Reset Silnik_prawy_b
     Pwm1b = 0 - V_p_temp
End If


 Pwm1a = V_l_temp                                          'Pwm1a => lewy
 Pwm1b = V_p_temp                                          'Pwm1b => prawy

Loop
End

Na tym kodzie silniki najpierw jechały do tyłu, musiałem zmienić odpowiednio "Set" i "Reset", gdy bot stoi środkowymi czujnikami nad linią nadal stoi. (nic nie piszczy) Testy robię przy Tp = 200.

Edit: Jak podłożyłem pod skrajne czujniki czarną linie to też silniki stoją - tak samo jak ze środkowymi i pewnie ze wszystkimi tak byłoby.

[Treker (Damian Szymański)]

Usuń na końcu:

  Pwm1a = V_l_temp                                          'Pwm1a => lewy 
 Pwm1b = V_p_temp                                          'Pwm1b => prawy 

Sprawdź jeszcze, co się dzieje, gdy Tp jest niskie typu 50.

[aixI]

No to jakaś magia... Wydaje mi się, że to kwestia czegoś "bascomowego". Nie piszę już w Bascomie i ciężko będzie znaleźć mi błąd.

No to trochę do kitu :/

Sprawdź jeszcze, co się dzieje, gdy Tp jest niskie typu 50?

Przy Tp = 50 też jest to samo, tylko wydaje mi się, że nawet silniki swojej prędkości nie zmieniają... jak zmieniam Tp.

Jak dałem Tp = 0, to silniki nadal jadą i zmiana Tp nic nie zmienia.

Edit:

Jak to usunąłem

  Pwm1a = V_l_temp                                          'Pwm1a => lewy 
  Pwm1b = V_p_temp                                          'Pwm1b => prawy

to silniki piszczą.

[Treker (Damian Szymański)]

Edit:

Jak to usunąłem

KOD PROGRAMU: ZAZNACZ CAŁY

   Pwm1a = V_l_temp                                          'Pwm1a => lewy

   Pwm1b = V_p_temp                                          'Pwm1b => prawy

to silniki piszczą.

Zmiana Tp wtedy też nic nie daje? Sprawdź dla 50 i 200.

Może w pętli gdzieś zerujesz te PWMy przez przypadek?

[aixI]

Kurcze wydaje mi się, że w pętli nigdzie się PWM'y nie zerują ;/

Nie wiem w czym byłoby lepiej pisać soft...w Bascom'ie...w C...(C dopiero się powoli uczę)

A prawie zapomniałbym, jak sobie kiedyś testowałem kod w C (algorytm P/PD dla lf'a) czujniki były odczytywane przez ADC, miałem 6 czujników i kiedy środkowymi czujnikami bot był na linii to jechał prosto 😃 Może pokusiłbym się o napisanie algorytmu w C (ale byłoby to baaardzo duże wyzwanie dla mnie) gdzie byłby odczyt czujników przez komparatory.

[Treker (Damian Szymański)]

Wklej cały kod jeszcze raz.

[aixI]

'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
Silnik_lewy_a Alias Portd.2
Silnik_lewy_b Alias Portd.3

Silnik_prawy_a Alias Portd.6
Silnik_prawy_b Alias Portd.7




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


Dim V_l_temp As Integer
Dim V_p_temp As Integer
Dim Tp As Byte


Dim X As Byte
Dim Roznica As Integer
Dim Uchyb_regulacji As Integer
Dim Zmiana As Single
Dim Kp As Integer
Dim Black As Byte
Dim Suma As Integer
Dim Aktualna_pozycja As Integer


Dim Czujniki(6) As Byte

Const Pozycja_docelowa = 0

Kp = 1


Tp = 127
V_l_temp = 0
V_p_temp = 0

Zmiana = 0


Set Led_red

  Set Silnik_lewy_a
  Reset Silnik_lewy_b

  Set Silnik_prawy_a
  Reset Silnik_prawy_b

Do


'==========================================
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
'============================================


  X = 1
  Black = 0
  Suma = 0

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

X = 0



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


V_l_temp = Tp + Zmiana                                      'lewy
V_p_temp = Tp - Zmiana                                      'prawy


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

If V_l_temp >= 0 Then
  Reset Silnik_lewy_a
  Set Silnik_lewy_b
     Pwm1a = V_l_temp
Else
  Set Silnik_lewy_a
  Reset Silnik_lewy_b
     Pwm1a = 0 - V_l_temp
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 V_p_temp >= 0 Then
  Reset Silnik_prawy_a
  Set Silnik_prawy_b
     Pwm1b = V_p_temp
Else
  Set Silnik_prawy_a
  Reset Silnik_prawy_b
     Pwm1b = 0 - V_p_temp
End If


'to jest "wyłączone"
'If Black >= 0 Then
 'Pwm1a = V_l_temp                                          'Pwm1a => lewy
 'Pwm1b = V_p_temp                                          'Pwm1b => prawy
'End If


Loop
End

Miałbyś możliwość przetestowania tego kodu w jednym ze swoich lf'ów, w których masz mikrokontroler AVR?

[Treker (Damian Szymański)]

Nie, niestety nie mam takiej możliwości. Nie mam nigdzie M16.

Powiedz jeszcze tylko, czy jak ustawisz normalnie robota na kartce i ustawisz na sztywno aby jechał z wypełnieniem PWM 75, to jedzie?

[aixI]

Wydaje mi się, że nie pojedzie, ponieważ nawet przy 127 ma problem.

Nie, niestety nie mam takiej możliwości. Nie mam nigdzie M16.

A jakby po prostu zmienić deklaracje procesora, wejść, wyjść i odpowiednich pinów?

[Treker (Damian Szymański)]

Ok, to był mój błąd miało być 127. No to w takim razie podnieś Tp do prędkości, która pozwala na płynna jazdę. Jaki ma się testowania czy robot jedzie, jeśli robisz to na wypełnienie, które jest zbyt małe.

Ja się trochę poddaje, nich ktoś inny świeżym okiem zerknie w kod i tak już sporo poprawiliśmy.