Das CD-Roboter-Prototyp hat heute die erste Probefahrt bestanden. Mit einem Aufsatz vorne bestehend aus zwei Photowiderständen, einem hellen LED und drei Widerständen konnte ich den Roboter mit der Linienverfolgefunktion ausstatten. Die Photowiderstände waren leider nicht vom gleichen Typ, so musste ich etwas messen und ausprobieren. Viele Schaltungen für die Liniendetektion benutzen Potentiometer. Ich hatte gerade keine zur Hand, aber Arduino kann ja auch selbst ausrechnen, was der Unterschied zwischen hell und dunkel ist. Wichtig ist nur, dass die Photowiderstände in einem möglichst linearen Bereich zwischen dem hellen und dunkleren Bereichen arbeiten.
Als Spannungsteiler nahm ich etwa gleich große Widerstandswerte, wie die Werte der LDRs bei dunkel.
Das LED befindet sich in der Mitte zwischen beiden LDRs. Diese Positionierung ist gut geeignet für eine helle Linie auf dunklem Hintergrund.
Vor der Fahrt wird der Roboter kalibriert. Nach dem ersten Blinken hat man 2 Sekunden Zeit den Roboter auf die Linie zu stellen. Die Helligkeit der Linie wird gemessen. Danach blinkt es 2-mal - man sollte den Roboter auf den Hintergrund stellen. Es wird die Helligkeit des Hintergrundes gemessen. Danach folgt 3-mal Blinken und nach 3 Sekunden (Zeit um den Roboter wieder auf die Linie zu stellen) fährt der Roboter los.
Auf dem Video sieht man, dass der Roboter die Linie aus der Mitte selbstständig findet. Leider hatte ich gerade keine dunklere Fläche als Pappkarton, so dass die Probefahrt im Dunkeln stattfinden sollte - damit der Kontrast zwischen hell und dunkel ausreichend war.
Das Algorithmus ist sehr einfach. Nach der Kalibrierung wird für jedes LDR ein Threshold für Dunkel ausgerechnet, das von mir willkürlich als die Mitte zwischen Hell- und Dunlkel definiert wurde.
Wenn die Werte beider LDRs unterhalb (hell) des Thresholds liegen, fährt der Roboter vorwärts. Wird an einer Seite dunkler (LDR Wert > Thresholds), gibt es eine leichte Drehung in die entgegengesetzte Richtung. Wenn beide LDRs zu dunkel messen, fährt der Roboter zurück mit leichter Drehung nach rechts.
Hier ist das Arduino -Quellcode:
/*
CD-Roboter Tests
Valentin Heinitz, 2014-10-30
http://heinitz-it.de
Die Software ist Public-Domain.
Einfacher Linienverfolger. Linie ist heller als sonstige Fläche.
Vor der Fahrt erfolgt die Kalibrierung - Aufnahme der Lichtintensitäten über der hellen und dunklen Flächen.
uC: Arduino Nano
Motorentreiber L293D
*/
void motors_stop() {
digitalWrite(2, HIGH);
digitalWrite(3, HIGH);
digitalWrite(4, HIGH);
digitalWrite(5, HIGH);
}
void motorR_fw() {
digitalWrite(3, HIGH);
digitalWrite(2, LOW);
}
void motorL_fw() {
digitalWrite(4, HIGH);
digitalWrite(5, LOW);
}
void motorR_bw() {
digitalWrite(2, HIGH);
digitalWrite(3, LOW);
}
void motorL_bw() {
digitalWrite(5, HIGH);
digitalWrite(4, LOW);
}
void turnRight( int ms)
{
motorR_bw();
motorL_fw();
delay(ms);
motors_stop();
}
void turnLeft( int ms)
{
motorR_fw();
motorL_bw();
delay(ms);
motors_stop();
}
void forward( int ms)
{
int steps = ms/20;
for( int i=0; i<steps; ++i)
{
digitalWrite(6, HIGH);
delayMicroseconds(10);
digitalWrite(6, LOW);
int distance = pulseIn(7, HIGH) / 2;
if ( distance < 700 )
{
break;
}
else
{
motorR_fw();
motorL_fw();
delay(20);
}
}
motors_stop();
}
void backward( int ms)
{
motorR_bw();
motorL_bw();
delay(ms);
motors_stop();
}
int DirMostRight=0;
int DirRight=1;
int DirFront=2;
int DirLeft=3;
int DirMostLeft=4;
int ObstacleRange=700;
int ShortThoughtTime=500;
int LongThoughtTime=2000;
int GoRange=1000;
int ranges[5]={0,0,0,0,0};
const int lrdLeft = A0;
const int lrdRight = A1;
int vLeft = 0;
int vRight = 0;
void checkObstacle()
{
turnRight(200);
digitalWrite(6, HIGH);
delayMicroseconds(10);
digitalWrite(6, LOW);
ranges[0] = pulseIn(7, HIGH) / 2;
delay(ShortThoughtTime);
turnLeft(100);
digitalWrite(6, HIGH);
delayMicroseconds(10);
digitalWrite(6, LOW);
ranges[1] = pulseIn(7, HIGH) / 2;
delay(ShortThoughtTime);
turnLeft(100);
digitalWrite(6, HIGH);
delayMicroseconds(10);
digitalWrite(6, LOW);
ranges[2] = pulseIn(7, HIGH) / 2;
delay(ShortThoughtTime);
turnLeft(100);
digitalWrite(6, HIGH);
delayMicroseconds(10);
digitalWrite(6, LOW);
ranges[3] = pulseIn(7, HIGH) / 2;
delay(ShortThoughtTime);
turnLeft(100);
digitalWrite(6, HIGH);
delayMicroseconds(10);
digitalWrite(6, LOW);
ranges[4] = pulseIn(7, HIGH) / 2;
delay(ShortThoughtTime);
turnRight(200);
}
int vleftRefLine=0;
int vleftRefOffroad=0;
int vrightRefLine=0;
int vrightRefOffroad=0;
int thleft=0;
int thright=0;
void setup() {
pinMode(2, OUTPUT);
pinMode(3, OUTPUT);
pinMode(4, OUTPUT);
pinMode(5, OUTPUT);
pinMode(6, OUTPUT);
pinMode(7, INPUT);
pinMode(8, OUTPUT);
motors_stop();
Serial.begin(9600);
Serial.write("CDRobot: Version 0.1\n");
Serial.write("Copyright: 2014, heinitz-it.de\n");
//delay(5000);
digitalWrite(8, HIGH); //1-Mal blinken: Aufnahme der Lichtintensitaet auf der Linie (Hell)
delay(100);
digitalWrite(8, LOW);
delay(2000);
digitalWrite(8, HIGH);
delay(300);
vleftRefLine = analogRead(lrdLeft);
vrightRefLine = analogRead(lrdRight);
digitalWrite(8, LOW);
delay(1000);
digitalWrite(8, HIGH); //2-Mal blinken: Aufnahme der Lichtintensitaet ausserhalb der Linie (dunkler)
delay(100);
digitalWrite(8, LOW);
delay(100);
digitalWrite(8, HIGH);
delay(100);
digitalWrite(8, LOW);
delay(2000);
digitalWrite(8, HIGH);
delay(300);
vleftRefOffroad = analogRead(lrdLeft);
vrightRefOffroad = analogRead(lrdRight);
digitalWrite(8, LOW);
delay(1000);
digitalWrite(8, HIGH); //3-Mal blinken: Auf die Linie stellen, gleich geht's los
delay(200);
digitalWrite(8, LOW);
delay(100);
digitalWrite(8, HIGH);
delay(200);
digitalWrite(8, LOW);
delay(100);
digitalWrite(8, HIGH);
delay(200);
digitalWrite(8, LOW);
delay(3000);
thleft = (vleftRefLine -vleftRefOffroad)/2 + vleftRefOffroad;
thright = (vrightRefLine -vrightRefOffroad)/2 + vrightRefOffroad;
vLeft = analogRead(lrdLeft);
vRight = analogRead(lrdRight);
Serial.print("Lth = " );
Serial.print(thleft);
Serial.print("\t Rth = ");
Serial.println(thright);
digitalWrite(8, HIGH);
}
void loop()
{
vLeft = analogRead(lrdLeft);
vRight = analogRead(lrdRight);
if( vLeft > thleft && vRight > thright )
{
backward(50);
turnRight(20);
}
else if( vLeft > thleft )
{
turnRight(30);
}
else if( vRight > thright )
{
turnLeft(30);
}
else
{
forward(30);
}
delay(10);
}
