Tesla - kolejna próba budowy linefollowera po latach

[mateusz797]

Ok a pokazałbys przykladowy program jak napiszesz? tak do wglądu zeby zobaczyc. Ja nigdy nie pisalem na PID

[MasterYoda95]

@mateusz797 Nie ma sprawy, wrzucę jak zdążę coś naskrobać.

[MasterYoda95]

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 Grudzień 23, 2020 przez MasterYoda95

[MasterYoda95]

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 Grudzień 24, 2020 przez MasterYoda95

[MasterYoda95]

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 Styczeń 1, 2021 przez MasterYoda95
dodałem filmy

[MasterYoda95]

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