Robosled z IR senzorji (QTR-8RC) in PID regulacijo
V izdelavi!
Za ugotavljanje in sledenje poti, črna proga na beli podlagi, bomo uporabili ploščico z osmimi IR senzorji QTR-8RC proizvajalca Pololu.
Slika 1: QTR-8RC senzorji (vir: https://www.pololu.com/picture/view/0J4642).
Potrebujemo še ploščico Arduino Uno, dva DC motorčka s kolesi in gonilnik L293.
Ploščica QTR-8RC vsebuje 8 IR (infra rdeči) oddajnih LED diod in v parih z njimi 8 sprejemnih fototranzistorjev. Čeprav ima QTR-8RC digitalne izhode, bomo mi uporabili analogne vhode na ploščici Arduino Uno, ker potrebujemo digitalne iizhode za gonilnik L293 (za določanje smeri vrtenja DC motorčkov). Za uporabo QTR-8RC senzorjev potrebujemo knjižnico za Arduino. Najdemo jo na tej strani, je pa tudi spodaj v prilogi. Ko smo dodali knjižnico, lahko v okolju Arduino kreiramo objekt za naš tip senzorjev QTR-8RC:
#include <QTRSensors.h>
#define NUM_SENSORS 8 //Število uporabljenih senzorjev.
#define TIMEOUT 2500 //Počakaj 2500 mikrosekund, da se postavijo izhodi senzorjev na LOW.
//Senzorji od 0 do 8 so priključeni na pine A0, A1, A2, A3, A4, A5, 4, 5
QTRSensorsRC qtrrc((unsigned char[]) {A0, A1, A2, A3, A4, A5, 4, 5},
NUM_SENSORS, TIMEOUT, 255);
unsigned int sensorValues[NUM_SENSORS];
Preden senzorje uporabimo, jih kalibriramo. Priključke senzorja QTR-8RC priključimo na Arduino Uno, kot prikazuje slika 2:
Slika 2: priključitev QTR-8RC na Arduino Uno
V programu za kalibracijo bomo vključili serijsko komunikacijo za prikaz vrednosti. Kalibriramo vedno pred začetkom vožnje, saj so vrednosti odvisne od različnih faktorjev, kot so svetloba, proga, oddaljenost senzorjev od podlage itd. Kalibriramo tako, da vseh 8 senzorjev premikamo čez belo in črno podlago na takšni oddaljenosti od podlage, kot jo bodo imeli senzorji med vožnjo. V času, ki ga imamo na razpolago za kalibracijo, sveti LED dioda, priključena na pin 13. Po končani kalibraciji LED dioda izklopi. Na serijskem monitorju bomo dobili izpisane maksimalne in mimimalne vrednosti, ki so odvisne od črno bele podlage. Nato se bodo vsakih 250 ms izpisovale vrednosti posameznih senzorjev ter pozicija.
//Senzor IR QTR-8RC, kalibriranje
//Avtor: Milan Ivič, februar 2016
#include <QTRSensors.h>
#define NUM_SENSORS 8 //Število uporabljenih senzorjev.
#define TIMEOUT 2500 //Počakaj 2500 mikrosekund, da se postavijo izhodi senzorjev na LOW.
//Senzorji od 0 do 8 so priključeni na pine:
QTRSensorsRC qtrrc((unsigned char[]) {A0, A1, A2, A3, A4, A5, 4, 5}, NUM_SENSORS, TIMEOUT, 255);
unsigned int sensorValues[NUM_SENSORS];
void setup()
{
digitalWrite(13, HIGH); //Indikacija, da smo v fazi kalibracije.
for (int i = 0; i < 200; i++) //Proces kalibracije traja cca. 5 sekund.
{
qtrrc.calibrate(); //Branje vseh senzorjev 10 krat.
}
digitalWrite(13, LOW); //Izklop LED, kalibracija zaključena.
Serial.begin(9600);
//Izpis minimalnih vrednosti:
Serial.print("Minimalne vrednosti: ");
for (int i = 0; i < NUM_SENSORS; i++)
{
Serial.print(qtrrc.calibratedMinimumOn[i]);
Serial.print(' ');
}
Serial.println();
//Izpis maksimalnih vrednosti:
Serial.print("Maksimalne vrednosti: ");
for (int i = 0; i < NUM_SENSORS; i++)
{
Serial.print(qtrrc.calibratedMaximumOn[i]);
Serial.print(' ');
}
Serial.println();
Serial.println();
delay(1000);
}
void loop()
{
unsigned int position = qtrrc.readLine(sensorValues);
for (unsigned char i = 0; i < NUM_SENSORS; i++)
{
Serial.print(sensorValues[i]);
Serial.print('\t'); //Ločeno s tabulatorjem.
}
Serial.println(position); //Izpis pozicije (od 0 do 7000).
delay(250);
}
Po kalibriranju se nam na serijskem monitorju najprej izpišejo minimalne in maksimalne vrednosti posameznih senzorjev:
Slika 3: Minimalne in maksimalne vrednosti posameznih senzorjev
Če premikamo ploščico s senzorji QTR-8RC čez belo in črno podlago, se nam izpisujejo vrednosti posameznih senzorjev, v skrajno desni koloni pa pozicija. Ker imamo ploščico z osmimi senzorji, je pozicija od 0 do 7000:
Slika 4: Vrednosti posameznih senzorjev in položaja pri premikanju QTR-8RC čez belo in črno podlago
Dodajmo sedaj dva DC (enosmerna) motorčka s kolesi in gonilnik L293, s pomočjo katerega krmilimo oba motorčka. Če menjamo polariteto priključene enosmerne napetosti, se motorček vrti v drugo smer. Za določanje smeri vrtenja (spreminjanje polaritete priključene napetosti in s tem spreminjanje smeri vrtenja DC motorčka) in ustavitev vrtenja uporabimo H-mostiček v integriranem vezju L293.
H-mostiček:
Princip delovanja vezja, po katerem ima H-mostiček ime vidimo na sliki 5.
Slika 5: Prikaz vezja za spreminjanje polaritete napetosti na priključkih DC motorčka, H-mostiček.
Vezje H-mostička vsebuje štiri stikala: A, B, C in D. S preklapljanjem teh stikal motorček krmilimo na različne načine:
Če vklopimo stikalo A in stikalo D, se bo motorček vrtel v smeri urinega kazalca,
Če vklopimo stikalo B in stikalo C, se bo motorček vrtel v nasprotni smeri urinega kazalca,
Če vklopimo stikalo A in stikalo B, se bo vrtenje motorčka ustavilo,
Če vsa stikala izklopimo, bo motorček brez napajalne napetosti,
Če istočasno vklopimo stikalo A in stikalo C ali stikalo B in stikalo D, je vezje v kratkem stiku, zato tega ne počnemo.
H-mostiček lahko izdelamo z različnimi elementi kot so releji, MOSFET, FET ali bipolarnimi BJT tranzistorji. Če pa naše zahteve po tokovni porabi niso prevelike, če želimo krmiliti majhne DC motorčke, ki ne potrebujejo večjih tokov, lahko enostavno uporabimo integrirano vezje L293.
Integrirano vezje L293:
Integrirano vezje L293 je namenjeno za krmiljenje dveh enosmernih motorčkov, da se lahko vrtita v obe smeri. Mi bomo krmilili dva motorčka. Nanj lahko priključimo motorčka, ki za svoje delovanje ne potrebujeta večjega toka od 1 A (L293D 600 mA) in za napetosti od 4,5 V do 36 V. Vsi vhodi so združljivi s TTL signali. Za zaščito ima L293 vgrajene hitre diode, ki ščitijo integrirano vezje pred napetostnimi konicami, ki nastanejo pri vklopu in izklopu motorčka (predvsem pri izklopu). Če se L293 segreje čez mejo 70°C, vgrajeni senzorji ustavijo delovanje motorčka.
Slika 6: Priključitev DC motorčkov na L293 (vir: Texas Instruments)
Desni motorček robosleda bomo priključili na pina 3 in 6, krmilili pa ga bomo s pini 1, 2 in 7. Na pin 8 (Vcc2) priključimo napetost, ki jo potrebuje DC motorček za delovanje, na pin 16 (Vcc1) pa napetost za gonilnik L293, ki znaša 5 V.
Levi motorček bomo priključili na pina 11 in 14, krmilili pa ga bomo s pini 9, 10 in 15.
Pina 1 in 9 sta enable pina. Nanju bomo pripeljali PWM signal (pulzno širinsko modulirani signal) od izhodov časovnika Timer2 razvojne plošče Arduino Uno. S pinami 1 in 2 ter 10 in 15 pa bomo krmilili smer vretenja desnega oziroma levega DC motorčka. Hitrost vrtenja motorčkov bomo torej spreminjali s PWM signalom.
Timer2 smo izbrali zato, ker je v Arduino Uno namenjen tonom, ki jih mi ne bomo uporabljali. Timer2 ima PWM izhoda na pinih 3 in 11 ploščice Arduino Uno. S spremembo določenih registrov, ki delujejo s Timer2, bomo spremenili frekvenco PWM signala, saj se DC motorčka bolje obnašata pri nižji frekvenci. Iz privzete frekvence 970 Hz jo bomo sremenili na frekvenco 61 Hz.
Pini 4 in 5 ter 12 in 13 morajo biti povezani na skupno maso (GND).
Pina 2 in 7 sta vhodna pina gonilnika L292 in sta kontrolna pina, ki nadzurujeta delovanje desnega motorčka.
Pina 10 in 15 sta vhodna pina gonilnika L292 in sta kontrolna pina, ki nadzurujeta delovanje levega motorčka.
Pina 3 in 6 sta izhodna pina gonilnika L293, kamor priključimo desni DC motorček.
Pina 11 in 14 sta izhodna pina gonilnika L293, kamor priključimo levi DC motorček.
Pin 16 je pin za napetostno napajanje gonilnika L293. Priključen mora biti na napetost +5 V
Pin 8 je pin, kamor priključimo pozitivno napetost iz drugega napajalnika oziroma baterije za napetost DC motorčka (od 4,5 V do 36 V, odvisno od uporabljenega motorčka). Mi bomo priključili napetost 9 V (baterija).
Posamezne elemente povežemo. Priključke ploščice QTR-8RC povežemo po zgornji sliki (slika 2), priključke gonilnika L293 pa po spodnji tabeli.
Sedaj lahko napišemo program, ki bo krmilil oba motorčka v odvisnosti od podlage (črne proge na beli podlagi). Program ni splošno uporaben, prikazuje pa princip delovanja.
//Robosled z uporabo senzorjev QTR-8RC in gonilnika L293.
//Avtor: Milan Ivič, februar 2016
#include <QTRSensors.h>
#define NUM_SENSORS 8 //Število uporabljenih senzorjev.
#define TIMEOUT 2500 //Počakaj 2500 mikrosekund, da se postavijo izhodi senzorjev na LOW.
//Senzorji od 0 do 8 so priključeni na pine:
QTRSensorsRC qtrrc((unsigned char[]) {A0, A1, A2, A3, A4, A5, 4, 5},
NUM_SENSORS, TIMEOUT, 255);
unsigned int sensorValues[NUM_SENSORS];
//Pini za gonilnik L293:
#define M_desniA 7 //Signal za desni motor, priključen na pin 2 na L293.
#define M_desniB 8 //Signal za desni motor, priključen na pin 7 na L293.
#define M_desniE 3 //Enable pin za desni motor, priključen na pin 1 na L293, PWM.
#define M_leviA 9 //Signal za levi motor, priključen na pin 10 na L293.
#define M_leviB 10 //Signal za levi motor, priključen na pin 15 na L293.
#define M_leviE 11 //Enable pin za levi motor, priključen na pin 9 na L293, PWM.
//Spremenljivki za PWM krmiljenje hitrosti vrenja DC motorčka (od 0 do 255):
int M_desni_Hitrost = 0; //Inicializacija.
int M_levi_Hitrost = 0;
void setup()
{
delay(500);
pinMode(13, OUTPUT);
pinMode(7, OUTPUT);
pinMode(8, OUTPUT);
pinMode(3, OUTPUT);
pinMode(9, OUTPUT);
pinMode(10, OUTPUT);
pinMode(11, OUTPUT);
digitalWrite(13, HIGH); //Indikacija, da smo v fazi kalibracije.
for (int i = 0; i < 200; i++) //Proces kalibracije traja cca. 5 sekund.
{
qtrrc.calibrate(); //Branje vseh senzorjev 10 krat.
}
digitalWrite(13, LOW); //Izklop LED, kalibracija zaključena.
//Izpis minimalnih vrednosti:
Serial.begin(9600);
for (int i = 0; i < NUM_SENSORS; i++)
{
Serial.print(qtrrc.calibratedMinimumOn[i]);
Serial.print(' ');
}
Serial.println();
//Izpis maksimalnih vrednosti:
for (int i = 0; i < NUM_SENSORS; i++)
{
Serial.print(qtrrc.calibratedMaximumOn[i]);
Serial.print(' ');
}
Serial.println();
Serial.println();
delay(1000);
//Sprememba frekvence PWM signala na Timer2:
TCCR2A = _BV(COM2A1) | _BV(COM2B1) | _BV(WGM21) | _BV(WGM20);
TCCR2B = _BV(CS20) | _BV(CS22);
}
void loop()
{
unsigned int position = qtrrc.readLine(sensorValues);
for (unsigned char i = 0; i < NUM_SENSORS; i++)
{
Serial.print(sensorValues[i]); //Izpisovanje vrednosti posameznih senzorjev.
Serial.print('\t');
}
Serial.print(position); //Izpisovanje pozicije robosleda.
delay(250);
unsigned int sensors[8];
int pozicija = qtrrc.readLine(sensors);
int error = pozicija - 3500; //error je v območju od -3500 do +3500.
Serial.print('\t');
Serial.print("Error: "); //Izpisovanje vrednosti errorja.
Serial.println(error);
if((error > -1500) && (error < 1500))
{
M_desni_Hitrost = 100; //Robosled nekje na sredini proge. Oba motorčka z enako hitrostjo.
M_levi_Hitrost = 100;
analogWrite(M_desniE, M_desni_Hitrost); //PWM za desni motor je 100 (hitrost).
digitalWrite(M_desniA, HIGH); //Vrtenje naprej.
digitalWrite(M_desniB, LOW);
analogWrite(M_leviE, M_levi_Hitrost); //PWM za levi motor je 100 (hitrost).
digitalWrite(M_leviA, HIGH); /Vrtenje naprej.
digitalWrite(M_leviB, LOW);
}
if(error < -1500) //Robosled na desni strani proge, zato se mora levi motorček ustaviti.
{
M_levi_Hitrost = 0;
M_desni_Hitrost = 100;
analogWrite(M_desniE, M_desni_Hitrost);
digitalWrite(M_desniA, HIGH);
digitalWrite(M_desniB, LOW);
analogWrite(M_leviE, M_levi_Hitrost);
digitalWrite(M_leviA, HIGH);
digitalWrite(M_leviB, LOW);
}
if(error > 1500) //Robosled na levi strani proge, zato se mora desni motorček ustaviti.
{
M_desni_Hitrost = 0;
M_levi_Hitrost = 100;
analogWrite(M_desniE, M_desni_Hitrost);
digitalWrite(M_desniA, HIGH);
digitalWrite(M_desniB, LOW);
analogWrite(M_leviE, M_levi_Hitrost);
digitalWrite(M_leviA, HIGH);
digitalWrite(M_leviB, LOW);
}
}
Na serijskem monitorju lahko opazujemo vrednosti senzorjev in velikost napake (error) v odvisnosti od položaja robosleda:
Slika 7: Izpis vrednosti senzorjev in velikost napake (error) v odvisnosti od položaja robosleda
Robosled bi po napisanem programu večinoma vozil cik cak. Program bi morali prilagoditi vsaki progi posebej, pa še bi bil odvisen od tipa motorčkov, velikosti koles, izrabe baterije itd. Ko bi posnemali določeno progo, bi morali upoštevati, da se pri blagih ovinkih eden od motorjev ne ustavi, temveč bi se vrtel z zmanjšano hitrostjo, ali pa drugi motor z večjo hitrostjo:
if(error > 1500) //Robosled na levi strani proge, zato se mora desni motorček vrteti počasneje.
{
M_desni_Hitrost = 30;
M_levi_Hitrost = 100;
analogWrite(M_desniE, M_desni_Hitrost);
digitalWrite(M_desniA, HIGH);
digitalWrite(M_desniB, LOW);
analogWrite(M_leviE, M_levi_Hitrost);
digitalWrite(M_leviA, HIGH);
digitalWrite(M_leviB, LOW);
}
Hitrost vrtenja lahko namreč spreminjamo v območju od 0 do 255 (PWM signal na enable pinu gonilnika L293).
Zato je veliko boljša PID regulacija, kjer PID skrbi za popravljanje smeri vožnje robosleda.
PID regulacija
Regulacija je zaprtozančno vodenje procesa, ki na osnovi primerjave dejanske in želene regulirane veličine na proces vpliva tako, da je napaka čim manjša. Poznamo dva načina delovanja regulacij: sledilno in regulacijsko. Pri sledilnem mora regulacijski sistem zagotoviti, da regulirana veličina dobro sledi referenci, spremembi nastavitvene točke (npr. primerno odzivanje DC motorčka ob spremembah nastavitvene točke). Pri regulacijskem delovanju pa je pomembno predvsem to, da regulator učinkovito odpravlja motnje.
Slika 8: Potek referenčne U(t) in regulirane y(t) veličine pri sledilni regulaciji
Proporcionalno-integralno-diferencialna regulacija ali regulacija PID je najbolj razširjena metoda v procesni industriji. Regulacija je izvedena kot vsota proporcionalnega (P), integralnega (I) in diferencialnega (D) člena. Člen P je neposredno odvisen od trenutne velikosti napake, člen I je odvisen od integrala preteklih napak, člen D pa od hitrosti spreminjanja napake, s katerim lahko deloma predvidimo prihodnje delovanje sistema.
Slika 9: Blok shema regulacije PID
KP = proporcionalni prirastek
KI = integralni prirastek
KD = diferencialni prirastek
e(t) = napaka, ki predstavlja razliko med nastavitveno vrednostjo in vrednostjo procesa, y(t)
Spodnja enačba opisuje delovanje regulacije PID:
Regulator PID ima tri nastavljive parametre: ojačanje KP, integralno časovno konstanto TI in konstanto diferenciranja TD. Referenčno veličino predstavlja nastavitvena vrednost (Set point), to je vrednost, ki jo želimo v našem procesu.
Slika 10: Proporcionalno, integralno in diferencialno delovanje
Pri regulatorju PID delujejo vsi trije členi istočasno, zato je odziv regulacije na spremembo nastavitvene točke rezultanta vseh treh členov.
Poglejmo program za PID regulacijo robosleda s senzorji QTR-8RC:
//PID regulacija vožnje robosleda z uporabo senzorjev QTR-8RC in gonilnika L293.
//Avtor: Milan Ivič, februar 2016
#include <QTRSensors.h>
#define NUM_SENSORS 8 //Število uporabljenih senzorjev.
#define TIMEOUT 2500 //Počakaj 2500 mikrosekund, da se postavijo izhodi senzorjev na LOW.
//Senzorji od 0 do 8 so priključeni na pine:
QTRSensorsRC qtrrc((unsigned char[]) {A0, A1, A2, A3, A4, A5, 4, 5},
NUM_SENSORS, TIMEOUT, 255);
unsigned int sensorValues[NUM_SENSORS];
//Pini za gonilnik L293:
#define M_desniA 7 //Signal za desni motor, priključen na pin 2 na L293.
#define M_desniB 8 //Signal za desni motor, priključen na pin 7 na L293.
#define M_desniE 3 //Enable pin za desni motor, priključen na pin 1 na L293, PWM.
#define M_leviA 9 //Signal za levi motor, priključen na pin 10 na L293.
#define M_leviB 10 //Signal za levi motor, priključen na pin 15 na L293.
#define M_leviE 11 //Enable pin za levi motor, priključen na pin 9 na L293, PWM.
int M_hitrost = 50;
//Parametri Kp, Ki in Kd:
//float Kp=50, Ki=0.001, Kd=1; //Nastavimo si različne vrednosti in opazujemo odzive.
float Kp=50, Ki=0.01, Kd=1;
float error=0, P=0, I=0, D=0, PID_vrednost=0; //Inicializacija.
float Prejsnji_error=0, Prejsnji_I=0;
void setup()
{
delay(500);
pinMode(13, OUTPUT);
pinMode(7, OUTPUT);
pinMode(8, OUTPUT);
pinMode(3, OUTPUT);
pinMode(9, OUTPUT);
pinMode(10, OUTPUT);
pinMode(11, OUTPUT);
digitalWrite(13, HIGH); //Indikacija, da smo v fazi kalibracije.
for (int i = 0; i < 200; i++) //Proces kalibracije traja cca. 5 sekund.
{
qtrrc.calibrate(); //Branje vseh senzorjev 10 krat.
}
digitalWrite(13, LOW); //Izklop LED, kalibracija zaključena.
//Izpis minimalnih vrednosti:
Serial.begin(9600);
for (int i = 0; i < NUM_SENSORS; i++)
{
Serial.print(qtrrc.calibratedMinimumOn[i]);
Serial.print(' ');
}
Serial.println();
//Izpis maksimalnih vrednosti:
for (int i = 0; i < NUM_SENSORS; i++)
{
Serial.print(qtrrc.calibratedMaximumOn[i]);
Serial.print(' ');
}
Serial.println();
Serial.println();
delay(1000);
//Sprememba frekvence PWM signala na Timer2:
TCCR2A = _BV(COM2A1) | _BV(COM2B1) | _BV(WGM21) | _BV(WGM20);
TCCR2B = _BV(CS20) | _BV(CS22);
}
void loop()
{
unsigned int position = qtrrc.readLine(sensorValues);
Serial.print("Pozicija: ");
Serial.print(position);
delay(250);
unsigned int sensors[8];
int pozicija = qtrrc.readLine(sensors);
error = pozicija - 3500; //error je v območju od -3500 do +3500.
error = error/1000; //Za lažje določanje spremenljivke PID_vrednost prestavimo error v območje od -3,5 do +3,5.
Serial.print('\t');
Serial.print("Error: "); //Izpisovanje error-jev na serijski monitor.
Serial.print(error);
P = error;
I = I + error;
D = error - Prejsnji_error;
PID_vrednost = (Kp*P) + (Ki*I) + (Kd*D); //Pri regulaciji PID delujejo vsi trije členi istočasno.
Prejsnji_error = error;
int M_desni_Hitrost = M_hitrost - PID_vrednost;
int M_levi_Hitrost = M_hitrost + PID_vrednost;
if(M_desni_Hitrost > 255) //Ostati moramo v območju od 0 do 255.
M_desni_Hitrost = 255;
if(M_desni_Hitrost < 0)
M_desni_Hitrost = 0;
if(M_levi_Hitrost > 255)
M_levi_Hitrost = 255;
if(M_levi_Hitrost < 0)
M_levi_Hitrost = 0;
Serial.print('\t');
Serial.print("M desni: ");
Serial.print(M_desni_Hitrost); //Izpisovanje hitrosti (PWM) desnega motorčka v odvisnosti od položaja na serijski monitor.
Serial.print('\t');
Serial.print("M levi: ");
Serial.println(M_levi_Hitrost); //Izpisovanje hitrosti (PWM) levega motorčka v odvisnosti od položaja na serijski monitor.
analogWrite(M_desniE, M_desni_Hitrost); //V odvisnosti od položaja se avtomatsko spreminja vrednost PWM signala.
analogWrite(M_leviE, M_levi_Hitrost); //V odvisnosti od položaja se avtomatsko spreminja vrednost PWM signala.
//Določanje smeri vrtenja motorčkov:
digitalWrite(M_desniA, HIGH);
digitalWrite(M_desniB, LOW);
digitalWrite(M_leviA, HIGH);
digitalWrite(M_leviB, LOW);
}
V odvisnosti od položaja robosleda na progi dobimo izpise na serijskem monitorju:
Slika 11: Izpis podatkov na serijskem monitorju, PID regulacija robosleda
Ob vsakem obhohu glavne zanke (void loop()) se pogleda, kakšna je pozicija robosleda na podlagi vrednosti posameznih senzorjev. Vrednost pozicije se lahko spreminja od 0 do 7000 (to je urejeno že v knjižnici QTRSensors.h in velja v primeru, ko uporabljamo vseh 8 senzorjev).
Spremenljivki error dodelimo najprej vrednost pozicije - 3500, ker je 3500 polovica razpona pozicije (7000/2 = 3500). Če bi bil robosled točno na sredini proge, bi bila vrednost error enaka 0 (3500 - 3500 = 0). Da dobimo manjša števila, te vrednosti delimo s 1000. Spremenljika error torej zavzema vrednosti od -3,5 do +3,5. Ker so to realna števila (števila z decimalno vejico), mora biti tip spremenljivke error float.
Na podlagi trenutnega in prejšnjega error-ja se ob vsakem obhodu glavne zanke izračunajo vrednosti P, I in D ter vrednost PID. Na pdlagi teh podatkov se neprestano spreminjata hitrosti vrtenja obeh motorčkov.
Če je PID_vrednost pozitivna, pomeni da se robosled nahaja levo od sredine proge, zato se moro položaj robosleda popraviti. Levi motorček se mora vrteti nekoliko hitreje, desni pa nekoliko počasneje:
int M_levi_Hitrost = M_hitrost + PID_vrednost;
int M_desni_Hitrost = M_hitrost - PID_vrednost;
Če je PID_vrednost negativna, pomeni da se robosled nahaja desno od sredine proge, zato se moro položaj robosleda popraviti. Desni motorček se mora vrteti nekoliko hitreje, levi pa nekoliko počasneje:
int M_levi_Hitrost = M_hitrost + PID_vrednost;
int M_desni_Hitrost = M_hitrost - PID_vrednost;
Maksimalni hitrost (M_hitrost) smo določili na začetku programa, v našem primeru je 50. To vrednost lahko spremenimo, odvisna je od uspešnosti vožnje robosleda po izbrani progi.
Vrednosti za levi (analogWrite(M_leviE, M_levi_Hitrost);) in desni (analogWrite(M_desniE, M_desni_Hitrost);) motorček določata PWM signal za oba enable pina gonilnika L293. Ker smo določili smer vrtenja obeh motorčkov, je hitrost vrtenja motorčkov odvisna od vrednosti PWM signala na obeh enable pinih gonilnika L293 (na pinu 1 za desni in na pinu 9 za levi motorček).
Kako določimo vrednosti Kp, Ki in Kd?
Ti podatki so najbolj pomembni v našem programu. Določimo jih lahko le s preiskušanjem, eksperimentiranjem. Te vrednosti so različne za vsak posamezni robosled in vsako posamezno konfiguracijo proge. Priporočam naslednjo metodo:
Ki in Kd postavimo na 0, Kp pa na 1 in opazujemo delovanje robosleda. Če se nam zdi da je robosled počasen, da vijuga po progi, povečamo vrednost Kp.
Ko je robosled dokaj stabilen na progi, dodelimo konstanti Kd vrednost 1, Ki pa vrednost 0,01 ali še manj in jo po potrebi povečujemo. Če bo Ki prevelik, se bo robosled sunkovito obračal levo in desno. Če bo Ki prenizek, pa ne bomo zaznali nobene razlike. Ki ima pomemben vpliv na vožnjo robosleda, zato ga nastavljamo po porakih za vrednost 0,01.
Ko robosled dokaj natančno sledi progi, povečujemo hitrost tako dolgo, da še sledi progi.
Želim vam veliko uspeha pri nastavitvah vašega robosleda!