Society of Robots - Robot Forum

Software => Software => Topic started by: saba_rish91 on January 31, 2012, 07:04:35 PM

Title: Please check my line fllower code
Post by: saba_rish91 on January 31, 2012, 07:04:35 PM
Hi,
My line fllower has 5 sensors and I use arduino uno.

Please chk the following code for errors.

Code: [Select]
int s1=2, s2=3, s3=4, s4=5, s5=6;
int v1,v2,v3,v4,v5;
int temp=0,v,b=0,aw=255;

void setup()
{
  pinMode(13,OUTPUT); //H-bridge
  pinMode(12,OUTPUT);  //H-bridge
  pinMode(8,OUTPUT);  //H-bridge
  pinMode(7,OUTPUT);  //H-bridge
  pinMode(2,INPUT);  //sensor 1
  pinMode(3,INPUT);  //sensor 2
  pinMode(4,INPUT);  //sensor 3
  pinMode(5,INPUT);  //sensor 4
  pinMode(6,INPUT);  //sensor 5
  pinMode(9,OUTPUT); //pwm
  pinMode(10,OUTPUT); //pwm
  //Serial.begin(9600);
}

void fwd()
{
    digitalWrite(13,HIGH);
    digitalWrite(12,LOW);
    digitalWrite(8,HIGH);
    digitalWrite(7,LOW);
}

void back()
{
    digitalWrite(13,LOW);
    digitalWrite(12,HIGH);
    digitalWrite(8,LOW);
    digitalWrite(7,HIGH);


void left()
{
    digitalWrite(13,LOW);
    digitalWrite(12,LOW);
    digitalWrite(8,HIGH);
    digitalWrite(7,LOW);
}

void right()
{
    digitalWrite(13,HIGH);
    digitalWrite(12,LOW);
    digitalWrite(8,HIGH);
    digitalWrite(7,HIGH);
}

void U_left()
{
    fwd();
    delay(150);
    com_stop();
    digitalWrite(13,LOW);
    digitalWrite(12,HIGH);
    digitalWrite(8,HIGH);
    digitalWrite(7,LOW);
    delay(510);
}

void U_right()
{
    fwd();
    delay(150);
    com_stop();
    digitalWrite(13,HIGH);
    digitalWrite(12,LOW);
    digitalWrite(8,LOW);
    digitalWrite(7,HIGH);
    delay(510);
}

void com_stop()
{
  digitalWrite(13,LOW);
  digitalWrite(12,LOW);
  digitalWrite(8,LOW);
  digitalWrite(7,LOW);
  digitalWrite(10,HIGH);
   
}
 
void sen_read()
{
  //+fwd();
  v1=digitalRead(s1);
  v2=digitalRead(s2);
  v3=digitalRead(s3);
  v4=digitalRead(s4);
  v5=digitalRead(s5);
  delay(15);
}

void nav_curve()
{
  sen_read();
  if((v1==HIGH)&&(v2==HIGH)&&(v3==LOW)&&(v4==HIGH)&&(v5==HIGH))
  {//11011
    fwd();
    //analogWrite(9,255);
    //analogWrite(10,255);
    delay(3);
  }
  else if((v1==HIGH)&&(v2==LOW)&&(v3==LOW)&&(v4==HIGH)&&(v5==HIGH))
  {//10011
    left();
   // analogWrite(9,150);
    //analogWrite(10,204);
    delay(3);
  }
  else if((v1==LOW)&&(v2==HIGH)&&(v3==HIGH)&&(v4==HIGH)&&(v5==HIGH))
  {//01111
    left();
    //analogWrite(9,250);
    //analogWrite(10,153);
    delay(23);
    analogWrite(9,230);
    nav_curve();
  }
  else if((v1==HIGH)&&(v2==LOW)&&(v3==HIGH)&&(v4==HIGH)&&(v5==HIGH))
  {//10111
    left();
    //analogWrite(9,204);
    //analogWrite(10,204);
    delay(3);
  }
   else if((v1==HIGH)&&(v2==HIGH)&&(v3==HIGH)&&(v4==LOW)&&(v5==HIGH))
  {//11101
    right();
    //analogWrite(9,204);
    //analogWrite(10,204);
    delay(3);
  }
  else if((v1==HIGH)&&(v2==HIGH)&&(v3==HIGH)&&(v4==HIGH)&&(v5==HIGH))
  {//11111
    fwd();
    //analogWrite(9,255);
    //analogWrite(10,255);
    delay(3);
  }
  else if((v1==HIGH)&&(v2==HIGH)&&(v3==LOW)&&(v4==LOW)&&(v5==HIGH))
  {//11001
    right();
    //analogWrite(9,204);
    //analogWrite(10,204);
    delay(3);
  }
  else if((v1==HIGH)&&(v2==HIGH)&&(v3==HIGH)&&(v4==HIGH)&&(v5==LOW))
  {//11110
    right();
    //analogWrite(9,153);
    //analogWrite(10,153);
    delay(23);
    analogWrite(9,230);
    nav_curve();
  }
}

void nav_turn()
{
  temp=0;
  if(((v1==LOW)&&(v2==LOW)&&(v3==LOW)&&(v4==HIGH)&&(v5==HIGH)))
  {
    fwd();
    delay(10);
    U_left(); 
  while(temp<5)
  {
    analogWrite(9,214);
    nav_curve();
    temp++;
  }
  }
  else if((v1==HIGH)&&(v2==HIGH)&&(v3==LOW)&&(v4==LOW)&&(v5==LOW))
  {//11000
  fwd();
    delay(10);
    U_right();
     
  while(temp<5)
  {
    analogWrite(9,214);
    nav_curve();
    temp++;
  }
  }
  else if(((v1==HIGH)&&(v2==HIGH)&&(v3==HIGH)&&(v4==LOW)&&(v5==LOW)))
  {
    fwd();
    delay(10);
    U_right();
     
  while(temp<5)
  {
    analogWrite(9,214);
    nav_curve();
    temp++;
  }
  }
  else if((v1==LOW)&&(v2==LOW)&&(v3==HIGH)&&(v4==HIGH)&&(v5==HIGH))
  {//00111
  fwd();
    delay(10);
    U_left();
     
  while(temp<5)
  {
    analogWrite(9,214);
    nav_curve();
    temp++;
  }
  }
  else if((v1==LOW)&&(v2==LOW)&&(v3==LOW)&&(v4==LOW)&&(v5==HIGH))
  {//00001
    fwd();
    delay(10);
    U_left();
     
  while(temp<5)
  {
    analogWrite(9,214);
    nav_curve();
    temp++;
  }
  }
  else if((v1==HIGH)&&(v2==LOW)&&(v3==LOW)&&(v4==LOW)&&(v5==LOW))
  {//10000
  fwd();
    delay(10);
    U_right();
    //analogWrite(9,126);
    //analogWrite(10,126);
    //delay(500);
  }
    else if((v1==LOW)&&(v2==LOW)&&(v3==LOW)&&(v4==LOW)&&(v5==LOW))
  {//00000
      com_stop();
      delay(5);
  }
  else
  {
    fwd();
    //analogWrite(9,150);
    //analogWrite(10,255);
    delay(3);
  }
}



void loop()
{
  analogWrite(9,255);
  nav_curve();
  analogWrite(9,255);
  nav_turn();
}


The lfr doesn't follow the line.
Please help.