Skocz do zawartości

LineFollower regulator PID - program


marcin123123

Pomocna odpowiedź

Witam! Mam problem z kodem do LF. Pisałem program bez regulatora PID i wszystko było ok, ale nie mogę zrobić tej implementacji PID, wszystko niby jest dobrze, ale niestety "nie jedzie". Proszę nie zwracać uwagi na połączenia elektryczne, wszystko jest na pewno dobrze połączone (procesor atmega8, czujniki podłączone pod porty cyfrowe). Poniżej kod:

#include <avr/io.h>
#include <util/delay.h>

#define Kp 1000
#define Ki 100
#define Kd 10000
#define ocr1a_max 250
#define ocr1b_max 250
#define ocr1a_base 100
#define ocr1b_base 100
#define offset 2

float position=0;


float read(void)
{

if((PINC&(1<<PC2))!=0) // czujnik SRODKOWY
{
	position=0;
}

if((PINC&(1<<PC3))!=0) // czujnik LEWY
{
	position=-1;
}

if((PINC&(1<<PC4))!=0) // czujnik LEWY SKRAJNY
{
	position=-2;
}

if((PINC&(1<<PC1))!=0) // czujnik PRAWY
{
	position=1;
}

if((PINC&(1<<PC0))!=0) // czujnik PRAWY SKRAJNY
{
	position=2;
}
return position;
}

int main(void)
{

float integral = 0;
float derivative = 0;
float lastError = 0;
float x=0;

DDRD |= (1<<PD0)|(1<<PD1); // lewy silnik, rejestr kierunku wyjsciowy
DDRD |= (1<<PD2)|(1<<PD3); // prawy silnik, rejestr kierunku wyjsciowy

DDRC &= ~(1<<PC2); // czujnik SRODKOWY, rejestr kierunku wejsciowy
DDRC &= ~(1<<PC3); // czujnik PRAWY, rejestr kierunku wejsciowy
DDRC &= ~(1<<PC0); // czujnik PRAWY SKRAJNY, rejestr kierunku wejsciowy
DDRC &= ~(1<<PC1); // czujnik LEWY, rejestr kierunku wejsciowy
DDRC &= ~(1<<PC4); // czujnik LEWY SKRAJNY, rejestr kierunku wejsciowy

DDRB |= (1<<PB1)|(1<<PB2); //pwm1,pwm2, rejestr kierunku wyjsciowy
TCCR1A |= (1<<COM1A1)|(1<<COM1B1)|(1<<WGM12)|(1<<WGM10); // tryb fast pwm 8-bit
TCCR1B |= (1<<CS12); // preskaler = 256

_delay_ms (2000);

while(1)
{
	x=read();
	float error = x - offset;
	integral = integral + error;
	derivative = error - lastError;
	float speed = Kp * error + Ki*integral + derivative * Kd;
	speed = speed/100;

	float ocr1a_speed = ocr1a_base + speed;
	float ocr1b_speed = ocr1b_base - speed;

	OCR1A=ocr1a_speed;
	OCR1B=ocr1b_speed;

	if (ocr1a_speed > ocr1a_max)
	{
		ocr1a_speed = ocr1a_max;
		OCR1A=ocr1a_max;

	}

	if (ocr1b_speed > ocr1b_max)
	{
		ocr1b_speed = ocr1b_max;
		OCR1B=ocr1b_max;

	}

	if (ocr1a_speed < 0)
	{
	ocr1a_speed = 0;
	OCR1A=ocr1a_speed;
	}

	if (ocr1b_speed < 0)
	{
	ocr1b_speed = 0;
	OCR1B=ocr1b_speed;
	}

	lastError = error;
	PORTD |=  (1<<PD0); PORTD &= ~(1<<PD1); // lewy przod
	PORTD |=  (1<<PD2); PORTD &= ~(1<<PD3); // prawy przod
	}

}


Nie wiem co może być przyczyną błędów w działaniu (robot dziwnie reaguje na linie lub w ogóle nie reaguje). Proszę o jakiekolwiek wskazówki.

Link do komentarza
Share on other sites

Dwie rzeczy, które się jako pierwsze rzucają w oczy:

- co ma robić offset i czemu jest równy 2?

- regulator PID powinien być wywoływany w stałych odstępach czasu, np co 10ms. U Ciebie jest po prostu wrzucony w pętlę, co w żaden sposób nie gwarantuje stałej częstotliwości wykonywania.

Link do komentarza
Share on other sites

1) Czemu wszystko robisz na floatach? Procesor wolniej posługuje się zmiennymi zmiennoprzecinkowymi niż całkowitymi. Bronić by mogło to dzielenie przez 100, ale równie dobrze możesz wszystkie współczynniki (Kp, Ki, Kd) podzielić przez 100, dalej będą całkowite i nie będzie dzielenia w programie.

2) Funkcja read jest trochę dziwna. Tworzysz zmienną globalną position, zapisujesz w niej wartości w funkcji, a potem ją zwracasz jako wynik? Niby jest poprawnie, ale czytelność taka sobie. Lepiej daj deklarację position w funkcji read.

3) Przy większej ilości czujników taka konstrukcja funkcji może nie wyrabiać, lepiej daać wagę do każdego czujnika (im dalej od środka, tym większa) i robić średnią czy coś takiego.

4) Zmiennej x mogłoby też w sumie nie być, lepiej daj error=read()-offset; zawsze się trochę miejsca zaoszczędzi 😋

A teraz co faktycznie może powodować problem:

5) Jak mactro napisał, regulator powinien być wywoływany co stały odstęp czasu i ten offset nie wiem czemu ma służyć

6) Współczynniki regulatora są trochę duże, sam człon D może się "nie zmieścić" w zakresie pwm. Wypadałoby też najpierw sprawdzać, czy prędkość nie jest za duża a dopiero potem wpisywać do pwm. Przy ujemnej prędkości możesz też odwracać polaryzację na silniku, żeby się kręcił w drugą stronę, tak jak "chce" regulator.

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

Przepraszam za długi czas bez odpowiedzi. Kombinowałem trochę i udało mi się stworzyć takie coś, jednak robot dalej nie chce jechać :/

#include <avr/io.h>
#include <util/delay.h>
#include <avr/interrupt.h>

#define Kp 10
#define Ki 1
#define Kd 100
#define ocr1a_max 250
#define ocr1b_max 250
#define ocr1a_base 100
#define ocr1b_base 100
#define offset 2

volatile float integral = 0;
volatile float derivative = 0;
volatile float lastError = 0;
volatile float speed;
volatile float ocr1a_speed;
volatile float ocr1b_speed;
volatile float error;


float read(void)
{
float position=0;
float liczba=0;

if((PINC&(1<<PC2))!=0) // czujnik SRODKOWY
{
	position=0;
	liczba++;
}

if((PINC&(1<<PC3))!=0) // czujnik LEWY
{
	position=-1;
	liczba++;
}

if((PINC&(1<<PC4))!=0) // czujnik LEWY SKRAJNY
{
	position=-2;
	liczba++;
}

if((PINC&(1<<PC1))!=0) // czujnik PRAWY
{
	position=1;
	liczba++;
}

if((PINC&(1<<PC0))!=0) // czujnik PRAWY SKRAJNY
{
	position=2;
	liczba++;
}
return position/liczba;
}

ISR(TIMER2_COMP_vect)
{
integral = integral + error;
derivative = error - lastError;
speed = Kp * error + derivative * Kd;
ocr1a_speed = ocr1a_base + speed;
ocr1b_speed = ocr1b_base - speed;
}

int main(void)
{

DDRD |= (1<<PD0)|(1<<PD1); // lewy silnik, rejestr kierunku wyjsciowy
DDRD |= (1<<PD2)|(1<<PD3); // prawy silnik, rejestr kierunku wyjsciowy

DDRC &= ~(1<<PC2); // czujnik SRODKOWY, rejestr kierunku wejsciowy
DDRC &= ~(1<<PC3); // czujnik PRAWY, rejestr kierunku wejsciowy
DDRC &= ~(1<<PC0); // czujnik PRAWY SKRAJNY, rejestr kierunku wejsciowy
DDRC &= ~(1<<PC1); // czujnik LEWY, rejestr kierunku wejsciowy
DDRC &= ~(1<<PC4); // czujnik LEWY SKRAJNY, rejestr kierunku wejsciowy


//ustawienie TIMER2
TCCR2 |= (1<<WGM21);
TCCR2 |= (1<<CS20)|(1<<CS21)|(1<<CS22);
OCR2=78;
TIMSK |= (1<<OCIE2);
sei();


//ustawienie TIMER1 do pwm
DDRB |= (1<<PB1)|(1<<PB2); //pwm1,pwm2, rejestr kierunku wyjsciowy
TCCR1A |= (1<<COM1A1)|(1<<COM1B1)|(1<<WGM12)|(1<<WGM10); // tryb fast pwm 8-bit
TCCR1B |= (1<<CS12); // preskaler = 256


_delay_ms (2000);

while(1)
{
	error = read() - offset;

	if (ocr1a_speed > ocr1a_max)
	{
		ocr1a_speed = ocr1a_max;
		OCR1A=ocr1a_max;
	}

	if (ocr1b_speed > ocr1b_max)
	{
		ocr1b_speed = ocr1b_max;
		OCR1B=ocr1b_max;
	}

	if (ocr1a_speed < 0)
	{
		ocr1a_speed = 0;
		OCR1A=ocr1a_speed;
	}

	if (ocr1b_speed < 0)
	{
		ocr1b_speed = 0;
		OCR1B=ocr1b_speed;
	}

	OCR1A=ocr1a_speed;
	OCR1B=ocr1b_speed;

	lastError = error;

	PORTD |=  (1<<PD0); PORTD &= ~(1<<PD1); // lewy przod
	PORTD |=  (1<<PD2); PORTD &= ~(1<<PD3); // prawy przod
}

}


Zmienna offset to zmienna mówiąca o największej możliwej odchyłce od położenia równowagi, podglądnąłem to stąd: http://www.inpharmix.com/jps/PID_Controller_For_Lego_Mindstorms_Robots.html .

Proszę o pomoc 😖

Link do komentarza
Share on other sites

Jeśli:

error = read() - offset;

oraz w funkcji "read()" przypisujesz wyjściu wartości od -2 do 2 dla czujników od lewego do prawego, to twój offset musi być równy zero. Jeśli jest on równy 2 to tak jakbyś kazał robotowi cały czas skręcać w lewo.

Link do komentarza
Share on other sites

I zmienna 'liczba' ustaw na razie do testów na sztywno 1. Tak jak masz napisane, to wydaje mi się, że ona cały czas będzie rosła, a ma tylko wskazywać ile czujników widzi linię.

Link do komentarza
Share on other sites

matty, akurat 'liczba' ma sens i nie będzie rosła bardziej niż do 5 (gdyby wszystkie czujniki widziały linię). Jest w końcu zerowana zaraz po wywołaniu funkcji read.

Zmienna 'offset' noże tu służyć jedynie do skorygowania jakiś mechanicznych niedoskonałości robota (np. silniki nierównej mocy), na skutek których, przy podaniu równego wypełnienia PWM na silniki, robot zamiast prosto skręcałby. Wtedy korygując wartość offset powinieneś móc doprowadzić do tego, że będzie jeździł prosto. Na razie ustaw ją jednak na 0.

Link do komentarza
Share on other sites

mactro, nie zauważyłem tego zerowania, mój błąd:) A co do offsetu to oryginalnie w regulatorze jest

error=wartosc_zadana-odczyt

, więc tutaj ten offset powinien być wartością zadaną. Korygowanie można dodać ewentualnie przy końcowym ustawieniu PWM.

Link do komentarza
Share on other sites

OK, teraz jest już dużo lepiej, robot podąża za linią, ale gdy nie zaden czujnik nie widzi linii robot zatrzymuje się. Próbowałem instrukcją typu

if(((PINC&(1<<PC0))==0) && ((PINC&(1<<PC1))==0) && ((PINC&(1<<PC4))==0) && ((PINC&(1<<PC3))==0) && ((PINC&(1<<PC2))==0))
{
               position=-2;
	liczba=5;
}

go nakierować na tor, ale dzieją się dziwne rzeczy.

Link do komentarza
Share on other sites

OK, teraz jest już dużo lepiej, robot podąża za linią, ale gdy nie zaden czujnik nie widzi linii robot zatrzymuje się.

Musisz zapamiętywać jaka wartość była ostatnio widziana. Gdy żaden czujnik nie widzi linii ustawiasz poprzednią wartość jako aktualną 🙂

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.