Joonejärgimisrobot

Lennart LüsiMaie-Liis Guitar

Allan IldKarl Leoste

Põhimõtted• Robot ei tohi oma tegevuse ega tegevusetusega

inimesele kahju teha;• Robot peab täitma inimese antud korraldusi, kui

need pole vastuolus esimese seadusega;• Robot peab kaitsma oma olemasolu, kuni see ei

lähe vastuollu esimese ega teise seadusega.

Eesmärgid• Robot järgib tumedat joont kuni joon eksisteerib • Joone puudumisel püüab robot põgeneda oma

loomulikku elukeskkonda.

Koodlong runningAverage(int M){ static int LM[100]; static byte index = 0; static long sum = 0; static byte count = 0; int LMSIZE = 50; sum -= LM[index]; LM[index] = M; sum += LM[index]; index = index % LMSIZE; if (count < LMSIZE) count++;

return sum / count;}

void drive(int spd, int trn = 0){ int lspd; int rspd; if (trn!=0){ lspd=spd-trn/2; rspd=spd+trn/2; } else { lspd=spd; rspd=spd; } if (rspd>0){ digitalWrite(13, HIGH); } else if(rspd<0){ digitalWrite(13, LOW); } if (lspd>0){ digitalWrite(12, LOW); } else if(lspd<0){ digitalWrite(12, HIGH);

} digitalWrite(9, LOW); //channel A nobreak digitalWrite(8, LOW); //channel B nobreak

if((spd!=0||trn!=0)&&abs(lspd)<minspeed){ lspd=minspeed; } if((spd!=0||trn!=0)&&abs(rspd)<minspeed){ rspd=minspeed; } if(lspd>maxspeed){ rspd=rspd-(lspd-maxspeed); lspd=maxspeed; } if(rspd>maxspeed){ lspd=lspd-(rspd-maxspeed); rspd=maxspeed; } analogWrite(3, abs(lspd)); //channel A speed 1-255 analogWrite(11, abs(rspd)); //channel B speed 1-255}

Veel koodiint readSensors(){ lightLevelv=analogRead(3); lightLevelp=analogRead(2); int avgleft=runningAverage(lightLevelv); int avgright=runningAverage(lightLevelp); int avgvahe=avgleft-avgright; /*Serial.print("readSensors[left:"); Serial.print(lightLevelv); Serial.print(" right:"); Serial.print(lightLevelp); Serial.print(" LeftAvg:"); Serial.print(avgleft); Serial.print(" RightAvg:"); Serial.print(avgright); Serial.print(" Avgvahe:"); Serial.println(avgvahe);*/ return(avgvahe);}

Olekumasinvoid loop(){ int avgvahe = readSensors(); switch (state) { case 0://State:STOP Serial.println("State:STOP"); drive(0,0); if((avgvahe > 2)||(avgvahe < 0)){ state = 2; //State:FOLLOWLINE } break; case 1: Serial.println("State:FORWARD"); speeed = 105;//State:FORWARD drive(speeed,0); if((avgvahe > 3)||(avgvahe < 1)){ state = 2; //State:FOLLOWLINE } break; case 2://State:FOLLOWLINE Serial.println("State:FOLLOWLINE");

if(avgvahe > 3){ drive(255,250); } else if(avgvahe < 1){ drive(255,-250); } else { state = 1;//State:FORWARD } break; default: Serial.println("State:default"); speeed = 255; drive(speeed,0); if(lightLevelv<whiteLimiter||lightLevelp<whiteLimiter){ state=2; } }}

Tänan kuulamast ja vaatamast!