hi
i found a code for line following and changed it to fit my project.
The movement control works have testet it. But the code to decide were the line is issent responding on the input it gets.
Help?
int qtiPin = 52; //c
int qti2Pin = 48; //h
int qti3Pin = 50; //v
int redbPin = 3; //led
int greenbPin = 4; //led
int redaPin = 11; //led
int greenaPin = 12;//led
int ledPin = 13; //led
int raw1, raw2, raw3;
boolean sen1, sen2, sen3;
int lastline;
long result = 0;
int lineplace;
int line;
int median = 40;//the point between the black and the white surface
void setup()
{
Serial.begin(9600);
pinMode(7,OUTPUT); //motorpins
pinMode(6,OUTPUT);
pinMode(8,OUTPUT);
pinMode(9,OUTPUT);
}
long RCtime(int sensPin)//operating for Qti sensors
{
long result = 0;
pinMode(sensPin, OUTPUT); // make pin OUTPUT
digitalWrite(sensPin, HIGH); // make pin HIGH to discharge capacitor
delay(1); // wait a ms to make sure cap is discharged
pinMode(sensPin, INPUT); // turn pin into an input and time till pin goes low
digitalWrite(sensPin, LOW); // turn pullups off - or it won't work
while(digitalRead(sensPin)){ // wait for pin to go low
result++;
}
return result; // report results
}
void fremad()//foraward
{
digitalWrite(7, LOW);
digitalWrite(6, HIGH);
digitalWrite(8, LOW);
digitalWrite(9, HIGH);
}
void bagud()//backwards
{
digitalWrite(7, HIGH);
digitalWrite(6, LOW);
digitalWrite(8, HIGH);
digitalWrite(9, LOW);
}
void hojrespin()//spin right
{
digitalWrite(7, LOW);
digitalWrite(6, HIGH);
digitalWrite(8, HIGH);
digitalWrite(9, LOW);
}
void venstrespin()//spinleft
{
digitalWrite(7, HIGH);
digitalWrite(6, LOW);
digitalWrite(8, LOW);
digitalWrite(9, HIGH);
}
void hojre()//right
{
digitalWrite(6, HIGH);
digitalWrite(7, LOW);
digitalWrite(8, LOW);
digitalWrite(9, LOW);
}
void venstre()//left
{
digitalWrite(7, LOW);
digitalWrite(6, LOW);
digitalWrite(8, LOW);
digitalWrite(9, HIGH);
}
void stans()//stop
{
digitalWrite(7, LOW);
digitalWrite(6, LOW);
digitalWrite(8, LOW);
digitalWrite(9, LOW);
}
int getline() //returns line position
{
raw1 = RCtime(qtiPin);
raw2 = RCtime(qti2Pin);
raw3 = RCtime(qti3Pin);
median = 40;//the point between the white and the black value
if (raw1 > median) //place the line "1" true(on line) "2" false(off line)
{
sen1 = 1;
}
if (raw1 < median)
{
sen1 = 0;
}
if (raw2 > median)
{
sen2 = 1;
}
if (raw2 < median)
{
sen2 = 0;
}
if (raw3 > median)
{
sen3 = 1;
}
if (raw3 < median)
{
sen3 = 0;
}
else
{
sen1 = 100;
}
//line placement
if (sen2 == 1)//højre/right
{
lineplace = -2;
}
if (sen1 == 1 && sen2 == 1)//mellem højre og center/between right and center
{
lineplace = -1;
}
if (sen1 == 1)//center/center
{
lineplace = 0;
}
if (sen1 == 1 && sen3 == 1)//mellem center og venstre/between center and left
{
lineplace = 1;
}
if (sen3 == 1)//venstre/left
{
lineplace = 2;
}
if (sen1 == 0 && sen2 == 0 && sen3)//væk/gone
{
lineplace = -10;
}
if (sen1 == 1 && sen2 == 1 && sen3 == 1)//over det hele/all black
{
lineplace = 10;
}
return lineplace;
}
void loop()
{
line = getline();
while (line == 0)
{
fremad();
lastline = 0;
digitalWrite(greenaPin, HIGH);
}
while (line == 1)
{
fremad();
lastline = 1;
}
while (line == -1);
{
fremad();
lastline = -1;
digitalWrite(redaPin, HIGH);
}
while (line == -2)
{
hojre();
lastline = -2;
digitalWrite(redaPin,HIGH);
}
while (line == 2)
{
venstre();
lastline = 2;
digitalWrite(redbPin,HIGH);
}
while (line == -10)
{
if (lastline == -2)
{
hojrespin();
digitalWrite(greenbPin,HIGH);
}
if (lastline == 2)
{
venstrespin();
digitalWrite(greenbPin,HIGH);
}
else
{
stans();
digitalWrite(redaPin, HIGH);
digitalWrite(redbPin, HIGH);
}
}
while (line == 10)
{
venstrespin();
}
{
Serial.print("sen1 ");
Serial.println(sen1);
Serial.print("sen2 ");
Serial.println(sen2);
Serial.print("sen3 ");
Serial.println(sen3);
Serial.print("raw1 ");
Serial.println(raw1);
Serial.print("raw2 ");
Serial.println(raw2);
Serial.print("raw3 ");
Serial.println(raw3);
Serial.print("line");
Serial.println(line);
}
}