Go Down

Topic: Line follower trouble (Read 1 time) previous topic - next topic

mmmm

Apr 05, 2013, 03:00 pm Last Edit: Apr 06, 2013, 12:44 am by mmmm Reason: 1
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?

Code: [Select]
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);
}
}

Go Up