Skocz do zawartości

Tesla - kolejna próba budowy linefollowera po latach


Pomocna odpowiedź

(edytowany)

Napisałem prosty regulator z członem proporcjonalnym, wklejam samą funkcję, bo cały kod byłby trudny do interpretacji. Funkcja wywoływana jest w pętli, odczytuje stan czujników za pomocą [czujnik].check() (zwraca 1 jeśli dany czujnik widzi linię i 0 gdy podłogę), wylicza błąd, mnoży go przez wagę czujnika - tym większą, im dalej od środka, znak mówi o kierunku. Następnie od wartości "normalnej" prędkości silnika (zmienna basespeed), którą przyjąłem na razie jako zaledwie 25%, odejmuje (lub dodaje w zależności od znaku) błąd pomnożony przez współczynnik Kp = 25 i wylicza nową prędkość dla silnika po tej stronie, po której pojawia się linia. Czyli na razie sprzężenie zwrotne jest ujemne i spowalniam silnik bliższy linii, a nie przyspieszam przeciwny. Wartości udało się dobrać już za drugim podejściem!

Na razie robot jest w stanie pokonywać łagodne zakręty. Na ostrzejszych niestety wypada. Wydaje mi się, że nie ma co majstrować z członem P i dodać człon różniczkujący. Przez święta może mi to trochę zająć. Wrzucam kiepskiej jakości filmik z sukcesem:

Obiecana funkcja:

Do obsługi silników przez mostek H napisałem sobie prostą klasę Motor, jeśli ktoś będzie chciał to wrzucę. W każdym razie silniki to rm (right motor) i lm (left motor), a funkcja .mv(x) tej klasy służy do zadawania ich prędkości w zakresie -100 (max do tyłu) do 100 (max do przodu). Myślę, że to będzie wygodny sposób zadawania prędkości przy wykorzystaniu regulatora PD.

def regulator():
    basespeed = 25
    global Kp
    readings = [LL.check(),L.check(),M.check(),R.check(),RR.check()]
    weights = [-2, -1, 0, 1, 2]
    error = 0
    counter = 0
    for i in range(len(weights)):
        if readings[i] == 1:
            error += weights[i]
            counter += 1
    if counter != 0:
        error = error/counter
        if error > 0:
            rmv = int(basespeed - Kp*error)
            rm.mv(rmv)
            lm.mv(basespeed)
            #uart.write('error: ' + str(error) + 'Lmv: ' + str(basespeed) +'Rmv: ' + str(rmv) + '\n\r')
            
        if error < 0:
            lmv = int(basespeed + Kp*error)
            lm.mv(lmv)
            rm.mv(basespeed)
            #uart.write('error: ' + str(error) + 'Lmv: ' + str(lmv) +'Rmv: ' + str(basespeed) + '\n\r')
    else:
        rm.mv(basespeed)
        lm.mv(basespeed)

 

Edytowano przez MasterYoda95
Link to post
Share on other sites
(edytowany)

Wielki sukces. Dokonałem tego, czego nie udało mi się dekadę temu!

Po dopisaniu członu różniczkującego robot już nie wypada z mojego toru!

Oczywiście nadal będę nad nim pracował, trzeba będzie zwiększyć prędkość, dodrukować obudowę trzymającą tę luźną płytkę i powerbank, niemniej podstawowe założenie zostało spełnione 🙂

Piękny prezent świąteczny sobie sprawiłem 😄 życzę wam równie pomyślnego konstruowania 🙂

Poniżej kod regulatora i nastawy. Z Kp zacząłem od wartości 1 i zadziałało od razu przy tej prędkości.

lasterror = (0,0) #error, timestamp
basespeed = 25
Kp = 25
Kd = 1

def regulator():
    global basespeed
    global Kp
    global Kd
    global lasterror
    readings = [LL.check(),L.check(),M.check(),R.check(),RR.check()]
    weights = [-2, -1, 0, 1, 2]
    error = 0
    counter = 0
    for i in range(len(weights)):
        if readings[i] == 1:
            error += weights[i]
            counter += 1
    if counter != 0:
        error = error/counter
        timenow = ticks_ms()
        derror = 0
        if error != lasterror[0] and lasterror[1] != 0:
            timenow = ticks_ms()
            dt = timenow - lasterror[1] + 1
            derror = (error - lasterror[0])/dt
            #uart.write("derror = {}, time difference = {}\n\r".format(derror,dt))
            
        lasterror = (error,timenow)
        if error > 0:
            rmv = int(basespeed - Kp*error - Kd*derror)
            rm.mv(rmv)
            lm.mv(basespeed)
            #uart.write('error: ' + str(error) + ' Lmv: ' + str(basespeed) +' Rmv: ' + str(rmv) + '\n\r')
            
        if error < 0:
            lmv = int(basespeed + Kp*error + Kd*derror)
            lm.mv(lmv)
            rm.mv(basespeed)
            #uart.write('error: ' + str(error) + ' Lmv: ' + str(lmv) +' Rmv: ' + str(basespeed) + '\n\r')
    else:
        rm.mv(basespeed)
        lm.mv(basespeed)

 

Edytowano przez MasterYoda95
  • Lubię! 1
Link to post
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

(edytowany)

Próby zwiększania prędkości robota kończyły się wypadaniem z toru na ostrzejszych zakrętach. Musiałem dopisać warunek, że jeśli linia ucieka z którejś strony poza zakres sensora, czyli na przykład ostatni odczyt błędu wynosi 2, a potem na czujnikach mam same zera, to ustawia się błąd 3. Dzięki temu robot robi jeszcze ostrzejszą korektę i wraca na tor.

Odkryłem też, że człon różniczkujący przy nastawie 1 nie robił zupełnie nic, bo poprawki jakie wnosił były rzędu 0.1 % szybkości silnika, a te procenty zaokrąglam do int-a, więc okrągłe 0 🙂 za to, że najpierw nie chciał działać sam człon P, a człon PD zadziałał musiała odpowiadać jakaś inna zmiana w kodzie, której nie mam jak wyśledzić. Do prędkości ok. 60% robot daje radę na samym regulatorze proporcjonalnym, potem coraz bardziej przydaje się człon różniczkujący z nastawą Kp= 100-300. Przy basespeed = 80 dochodzę do maksa możliwości, robot często wyjeżdża linią poza zakres czujników i nie zawsze wyrabia się z powrotem.

Sądziłem że przy tych chińskich silniczkach i twardych kółkach 5 czujników wystarczy, ale jednak całkiem nieźle rozpędzają robota i przydałoby się te 8 czujników.

Wrzucam wersję 2.0 kodu regulatora.

lasterror = (0,0) #error, timestamp
basespeed = 80
Kp = 30
Kd = 200
prevval = []

def regulator():
    global basespeed
    global Kp
    global Kd
    global lasterror
    readings = [LL.check(),L.check(),M.check(),R.check(),RR.check()]
    #uart.write(str(readings) + '\n\r')
    weights = [-2, -1, 0, 1, 2]
    error = 0
    counter = 0
    for i in range(len(weights)):
        if readings[i] == 1:
            error += weights[i]
            counter += 1
    if counter != 0:
        error = error/counter
    elif counter == 0 and (lasterror[0] == -2 or lasterror[0] == -3):
        error = -3
    elif counter == 0 and (lasterror[0] == 2 or lasterror[0] == 3):
        error = 3
    else:
        error = 0
    
    timenow = ticks_ms()
    derror = 0
    if error != lasterror[0] and lasterror[1] != 0:
        timenow = ticks_ms()
        dt = timenow - lasterror[1] + 1
        derror = (error - lasterror[0])/dt
        if debugflag:
            x = Kd*derror
            uart.write("Differential correction = {}\n\r".format(x))
        
    lasterror = (error,timenow)
    if error == 0:
        rm.mv(basespeed)
        lm.mv(basespeed)
        if debugflag:
            uart.write('error: ' + str(error) + ' Lmv: ' + str(basespeed) +' Rmv: ' + str(basespeed) + '\n\r')
            
    if error > 0:
        rmv = int(basespeed - Kp*error - Kd*derror)
        rm.mv(rmv)
        lm.mv(basespeed)
        if debugflag:
            uart.write('error: ' + str(error) + ' Lmv: ' + str(basespeed) +' Rmv: ' + str(rmv) + '\n\r')
        
    if error < 0:
        lmv = int(basespeed + Kp*error + Kd*derror)
        lm.mv(lmv)
        rm.mv(basespeed)
        if debugflag:
            uart.write('error: ' + str(error) + ' Lmv: ' + str(lmv) +' Rmv: ' + str(basespeed) + '\n\r')

Zaprojektuję jeszcze obudowę, aby płytka i powerbank stabilnie trzymały się podstawy robota. Trochę jestem ciekawy, czy odkąd przeniosłem projekt na płytkę drukowaną, eliminując część dodatkowych rezystancji na konektorach kabli, to czy układ pociągnąłby na bateriach, które stosowałem na początku. Z drugiej strony powerbank, mimo iż jakieś chińskie dziadostwo, spisuje się bardzo dobrze, więc nie wiem czy chce mi się znowu lutować kable koszyka baterii 🙂

Edit: dodaję filmiki, przepraszam że tak ciemno, dla ludzkiego oka było sporo jaśniej, ale mój telefon widział to inaczej 😞 nie będę już nagrywał i wrzucał od nowa, widać samo zachowanie robota.

1. Przejazd przy prędkości bazowej 70%

 

2a, 2b. Problemy przy prędkości 80%

 

 

 

Edytowano przez MasterYoda95
dodałem filmy
  • Lubię! 1
Link to post
Share on other sites

@Treker na tę chwilę zakończyłem rozwój tego projektu. Czy mógłbyś przenieść go z "w budowie" do odpowiedniego działu?

Link to post
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.