Rad efterföljare Robot - PID Control - Android Setup (6 / 12 steg)
Steg 6: Genomföra Sensor logiken
IR-sensorn består av en enskild IR-LED och en IR fotodiod. IR-ljus som avges av LED slår ytan och reflekteras tillbaka till IR fotodiod. Fotodiod ger sedan en tillverkad spänning proportionell mot reflektans av ytan (högt värde för ljus yta) och låg för svart/mörk yta. När det gäller den sensorer används, en integrerad krets på modulen genereras som utdata en enkel digital signal (hög: mörk; LÅG: Ljus). En potentiometer installerat på modulen (se bild) kommer att justera rätt nivå av ljus för att anses vara "mörka" eller inte. Det fungerar på ett sätt att när den reflekterade ljus färgen är svart/mörk, ("1") hög genereras på sin produktion och en låg ("0") för en annan ljusare färg. Jag använde här en integrerad modul med 4 sensorer och och extra modul med en enda sensor (annan form, men samma logik). Kombinationen är en matris med 5 sensorer som jag tycker är bra för en fin kontroll som förklaras nedan.
Rad 5 sensorer är monterad på ett sätt att om bara en sensor är centrerad avseende den svarta linjen, bara att särskilda sensorn kommer att producera en hög. Av andra sidan, bör avståndet mellan sensorer beräknas för att tillåta att 2 sensorer kan täcka den fulla bredden av den svarta linjen samtidigt, producerar också en hög på båda sensorerna (se bilder här).
Arrayen möjliga sensor utdata när du följer en linje är:
- 0 0 0 0 1
- 0 0 0 1 1
- 0 0 0 1 0
- 0 0 1 1 0
- 0 0 1 0 0
- 0 1 1 0 0
- 0 1 0 0 0
- 1 1 0 0 0
- 1 0 0 0 0
Med 5 sensorer, tillåter en generation av en "fel variabel" som hjälper till att styra robotens läge över linjen, som visas nedan.
Låt oss betrakta att det optimala villkoret är när roboten är centrerad, med linjen bara vråla "mellersta sensor" (Sensor 2). Produktionen av matrisen kommer att vara: 0 0 1 0 0 och i denna situation, "fel" kommer att "noll". Om roboten börjar till drivna till vänster (linje "verkar flytta" rätt") måste felet öka med en positiv signal. Om roboten börjar flytta åt höger (linje "verkar flytta" vänster"), på samma sätt, måste felet öka, men nu med en negativ signal.
Variabeln fel med med sensor status kommer att vara:
- 0 0 0 0 1 == > fel = 4
- 0 0 0 1 1 == > fel = 3
- 0 0 0 1 0 == > fel = 2
- 0 0 1 1 0 == > fel = 1
- 0 0 1 0 0 == > fel = 0
- 0 1 1 0 0 == > fel = -1
- 0 1 0 0 0 == > fel = -2
- 1 1 0 0 0 == > fel = -3
- 1 0 0 0 0 == > fel = -4
Titta på Arduino koden, var och en av sensorerna kommer att definieras med ett visst namn (anser att linjen följa sensorn mer till vänster måste tilldelas med en etikett "0"):
CONST int lineFollowSensor0 = 12;
CONST int lineFollowSensor1 = 18;
CONST int lineFollowSensor2 = 17;
CONST int lineFollowSensor3 = 16.
CONST int lineFollowSensor4 = 19.
Lagring för att kommer värdena för varje sensor en array variabel skapas:
int LFSensor [5] = {0, 0, 0, 0, 0};
Varje position i matrisen uppdateras ständigt med produktionen av var och en av sensorerna:
LFSensor [0] = digitalRead(lineFollowSensor0);
LFSensor [1] = digitalRead(lineFollowSensor1);
LFSensor [2] = digitalRead(lineFollowSensor2);
LFSensor [3] = digitalRead(lineFollowSensor3);
LFSensor [4] = digitalRead(lineFollowSensor4);
Med värdet av var och en av sensorerna, måste en logik genomföras för att generera fel variabeln:
om ((LFSensor [0] == 0) & & (LFSensor [1] == 0) & & (LFSensor [2] == 0) & & (LFSensor [3] == 0) & & (LFSensor [4] == 1)) fel = 4;
annars om ((LFSensor [0] == 0) & & (LFSensor [1] == 0) & & (LFSensor [2] == 0) & & (LFSensor [3] == 1) & & (LFSensor [4] == 1)) fel = 3;
annars om ((LFSensor [0] == 0) & & (LFSensor [1] == 0) & & (LFSensor [2] == 0) & & (LFSensor [3] == 1) & & (LFSensor [4] == 0)) fel = 2;
annars om ((LFSensor [0] == 0) & & (LFSensor [1] == 0) & & (LFSensor [2] == 1) & & (LFSensor [3] == 1) & & (LFSensor [4] == 0)) fel = 1;
annars om ((LFSensor [0] == 0) & & (LFSensor [1] == 0) & & (LFSensor [2] == 1) & & (LFSensor [3] == 0) & & (LFSensor [4] == 0)) fel = 0;
annars om ((LFSensor [0] == 0) & & (LFSensor [1] == 1) & & (LFSensor [2] == 1) & & (LFSensor [3] == 0) & & (LFSensor [4] == 0)) fel =-1;
annars om ((LFSensor [0] == 0) & & (LFSensor [1] == 1) & & (LFSensor [2] == 0) & & (LFSensor [3] == 0) & & (LFSensor [4] == 0)) fel = -2;
annars om ((LFSensor [0] == 1) & & (LFSensor [1] == 1) & & (LFSensor [2] == 0) & & (LFSensor [3] == 0) & & (LFSensor [4] == 0)) fel = -3;
annars om ((LFSensor [0] == 1) & & (LFSensor [1] == 0) & & (LFSensor [2] == 0) & & (LFSensor [3] == 0) & & (LFSensor [4] == 0)) fel = -4;