LineFollower regulator PID - program

marcin123123 · Luty 19, 2015

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.

mactro · Luty 19, 2015

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.

Wojcik98 · Luty 19, 2015

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.

matty · Luty 19, 2015

Dodatkowo na początku wyłącz całkiem całkowanie, bo mogą się różne, dziwne rzeczy dziać. Jak PD zacznie działać to dopiero próbuj I dodawać.

marcin123123 · Luty 23, 2015

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

Wojciech · Luty 23, 2015

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.

matty · Luty 24, 2015

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ę.

mactro · Luty 24, 2015

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.

matty · Luty 24, 2015

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.

marcin123123 · Luty 24, 2015

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.

Treker · Marzec 10, 2015

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ą

LineFollower regulator PID - program

Pomocna odpowiedź

marcin123123

mactro

Wojcik98

matty

marcin123123

Wojciech

matty

mactro

matty

marcin123123

Treker (Damian Szymański)

Bądź aktywny - zaloguj się lub utwórz konto!

Utwórz konto w ~20 sekund!

Zaloguj się

Quizy

Najnowsze posty w innych tematach

Przetwornica 12-230 V. Ogólne uwagi co do poprawności schematu 1 2 3 4 11

Sztuczne obciazenie DIY 1 2 3 4 6

Kurs Arduino - #8 - Sterowanie silnikami DC, pętla for 1 2 3 4 19

protokol onewire...kto slucha, kto nadaje? 1 2 3

Kurs STM32L4 – #18 – podsumowanie kursu, quiz 1 2

Inne