Need help for mapping and edge detection for traversing of my robot

Community General Discussions
1

My bot consists of 3 IR sensor,,,two fixed in two sides f the front and one on the servomotor at the middle top. I wrote this arduino program to detect a gap among some obstacles for my bot to pass through. Please tel me where i am going wrong or whether my logic s correct or not???

 

#include  <Servo.h>

 

Servo myservo;

int pos=0;

int dist[17];

 

int STBY = 10; 

int PWMA = 3; 

int AIN1 = 9; 

int AIN2 = 8; 

int PWMB = 5;

int BIN1 = 11;

int BIN2 = 12;

int sensorpin1 = 0;

int sensorpin2 = 5;

int sensorpin3 = 5;

 

void setup()

{

  myservo.attach(9);

  Serial.begin(9600);

 

   pinMode(STBY,OUTPUT);

   pinMode(PWMA,OUTPUT);

   pinMode(AIN1,OUTPUT);

   pinMode(AIN2,OUTPUT);

   pinMode(PWMB,OUTPUT);

   pinMode(BIN1,OUTPUT);

   pinMode(BIN2,OUTPUT);

}

 

int obstacle=20;

int direction();

int distance(int val);

void loop()

{

   int d;

   scan();

  /* d=direction();

   switch(d)

   {

     case 1: left();

             break;

     case 2: forward();

             break;

     case 3: right();

             break;

     case 4: ultimatebackward();

             break;

   }*/         

  }

  void scan()

  {

    int i;

    for(pos=0, i=0; pos<=180; pos+=10,i++)

   {

     myservo.write(pos);

 

    dist[i]=distance(analogRead(sensorpin3));

     delay(100);

     if(dist[i]<20)

     {

       dist[i]=0;

     }

     else

     dist[i]=1;

    }

    for(pos=180,i=17; pos>=0; pos-=10,i--)

   {

     myservo.write(pos);

 

     dist[i]=distance(analogRead(sensorpin3));

      delay(100);

     if(dist[i]<20)

     {

       dist[i]=0;

     }

      else

     dist[i]=1;

    }

  }

  int direction()

  {

 

     int count1,count2,newcount,pos1,pos2;

        for(int i=1;i<=17;i++)

        {

          if(dist[i]==0)

          {

            if(dist[i-1]==0)

            {

              count1++;

            }

            else

            {

              pos=i;

              count2=count1;

              count1=0;

              count1++;

               if(count2>newcount)

                  {

                    newcount=count2;

                    count2=0;

                  }

            }

 

          }

          pos2=pos1+newcount-1;

        if((0<=pos1<=5)&&(0<=pos2<=5))

        { 

          return(1);

        }

        else

        if((6<=pos1<=11)&&(6<=pos2<=11))

        {

          return(2);

        }

        else

        if((12<=pos1<=17)&&(12<=pos2<=17))

        {

          return(3);

        }

        else

        if((0<=pos1<=5)&&(6<=pos2<=11)&&((5-pos1)<(pos2-5)))

        {

          return(2);

        }

        else

         if((0<=pos1<=5)&&(6<=pos2<=11)&&((5-pos1)>(pos2-5)))

        {

          return(1);

        }

        else

         if((5<=pos1<=11)&&(12<=pos2<=17)&&((11-pos1)>(pos2-11)))

        {

          return(1);

        }

        else

         if((5<=pos1<=11)&&(12<=pos2<=17)&&((11-pos1)<(pos2-11)))

        {

          return(3);

        }

        else 

        return(4);

     }

 

     }

     int distance(int val)

       {

         int dis;

      //dis=(2914 / (val + 5)) - 1;

        dis=2076/(val-11);

     //Serial.println(dis);

       return dis;

      }

   void forward()

     {

       int dist2,dist3;

       dist2=analogRead(sensorpin1);

       while(obstacle>dist2)

       {

         stop();

         backward();

         right();

       } 

       dist3=analogRead(sensorpin2);

       while(obstacle>dist3)

       {

         stop();

         backward();

         left();

       } 

       digitalWrite(STBY,HIGH);

 

       digitalWrite(AIN1,LOW);

       digitalWrite(AIN2,HIGH);

       digitalWrite(BIN1,LOW);

       digitalWrite(BIN2,HIGH);

 

       analogWrite(PWMA,200);

       analogWrite(PWMB,200);

       delay(500);

     } 

 

  void backward()

     {

       digitalWrite(STBY,HIGH);

 

       digitalWrite(AIN1,HIGH);

       digitalWrite(AIN2,LOW);

       digitalWrite(BIN1,HIGH);

       digitalWrite(BIN2,LOW);

 

       analogWrite(PWMA,200);

       analogWrite(PWMB,200);

       delay(250);

     }

 

  void right()

     {

       digitalWrite(STBY,HIGH);

 

       digitalWrite(AIN1,LOW);

       digitalWrite(AIN2,HIGH);

       digitalWrite(BIN1,HIGH);

       digitalWrite(BIN2,LOW);

 

       analogWrite(PWMA,200);

       analogWrite(PWMB,200);

       delay(500);

       stop();

    }

 

  void left()

 

   {

       digitalWrite(STBY,HIGH);

 

       digitalWrite(AIN1,HIGH);

       digitalWrite(AIN2,LOW);

       digitalWrite(BIN1,LOW);

       digitalWrite(BIN2,HIGH);

 

       analogWrite(PWMA,200);

       analogWrite(PWMB,200);

       delay(500);

       stop();

   }   

   void ultimatebackward()

   {

      digitalWrite(STBY,HIGH);

 

       digitalWrite(AIN1,HIGH);

       digitalWrite(AIN2,LOW);

       digitalWrite(BIN1,HIGH);

       digitalWrite(BIN2,LOW);

 

       analogWrite(PWMA,200);

       analogWrite(PWMB,200);

       delay(250);

 

       digitalWrite(AIN1,HIGH);

       digitalWrite(AIN2,LOW);

       digitalWrite(BIN1,LOW);

       digitalWrite(BIN2,HIGH);

       analogWrite(PWMA,200);

       analogWrite(PWMB,200);

       delay(2000);

 

   }    

 

   void stop()

   {

       digitalWrite(STBY,LOW);

       delay(10);

 

   } 

2

I admit to having not programmed for a bit, but,
I don’t think you can do the if (23<=pos1<=54) etc etc. I believe you must break that up as (23 <= pos1 || pos1 <= 54). I could be wrong, and you can bet that if I am someone smarter will set me straight.

3

Looking over your code some more,
I believe your scan() is broken. I am unsure if you can overload the for() loop the way you have.
Stupid me. Your loop() calls scan(). scan() doesn’t call anything else.
I modified your code some, and, had remarked some bits out. I decided to remove everything that is rem’d out. All your program will ever do is scan(), if you can overload the for() loop as you have.
http://pastebin.com/RhCfU69v

4

Thanks dude…lot f help. I

Thanks dude…lot f help. I made a few changes from your edits. The situation has improved but there is no desired results. The bot keeps moving backward , that also only motors on rightside move back. This is my present code:

#include  <Servo.h>

 

Servo myservo;

int pos=0;

int dist[19];

 

int STBY = 10;

int PWMA = 3;

int AIN1 = 9;

int AIN2 = 8;

int PWMB = 5;

int BIN1 = 11;

int BIN2 = 12;

int sensorpin1 = 0;

int sensorpin2 = 3;

int sensorpin3 = 5;

 

void setup()

{

  myservo.attach(9);

  Serial.begin(9600);

 

  pinMode(STBY,OUTPUT);

  pinMode(PWMA,OUTPUT);

  pinMode(AIN1,OUTPUT);

  pinMode(AIN2,OUTPUT);

  pinMode(PWMB,OUTPUT);

  pinMode(BIN1,OUTPUT);

  pinMode(BIN2,OUTPUT);

}

 

int obstacle=20;

int direction();

int distance(int val);

void loop()

{

  int d;

  scan();

  d=direction();

  //Serial.println(d);

  switch(d)

  {

  case 1:

    left();

    break;

  case 2:

    forward();

    break;

  case 3:

    right();

    break;

//  case 4: //ultimatebackward();

//    stop();

//    backward();

//    break;

  }

}

void scan()

{

  int i;

  for(pos=10, i=0; pos<180; pos+=10,i++)

  {

    myservo.write(pos);

 

    dist[i]=distance(analogRead(sensorpin3));

 

    delay(50);

    if(dist[i]<20)

    {

      dist[i]=1;

    }

    else

    {

      dist[i]=0;

    }

    //Serial.println(dist[i]);

  }

 

  for(pos=180,i=17; pos>=10; pos-=10,i–)

  {

    // myservo.write(180);

    myservo.write(pos);

 

 

    dist[i]=distance(analogRead(sensorpin3));

    // Serial.println(dist[i]);

    delay(50);

 

    if(dist[i]<20)

    {

      dist[i]=1;

    }

    else

    {

      dist[i]=0;

    }

    // Serial.println(dist[i]);

  }

  //delay(50);

 

}

int direction()

{

  dist[0]=1;

  int count1,count2,newcount,pos1,pos2,dir;

  count1=0;

  count2=0;

  dir=-1;

  pos1=0;

  pos2=0;

  newcount=0;

  for(int i=1;i<18;i++)

  {

    if(dist[i]==0)

    {

      if(dist[i-1]==0)

      {

        count1++;

 

      }

      else

      {

        pos1=i;

        count2=count1;

        count1=0;

        count1++;

        if(count2>newcount)

        {

          newcount=count2;

          count2=0;

        }

      }

//      if(i=17)

//      {

//        count2=count1;

//        count1=0;

//        if(count2>newcount)

//        {

//          newcount=count2;

//          count2=0;

//        }

//        count2=0;

//      }

    }

  }

 // if(newcount>5)

  {

    //Serial.println(pos1);

    //delay(100);

    //Serial.println(newcount);

    pos2=pos1+newcount-1;

    //Serial.println(pos2);

    if((0<=pos1)&&(pos1<=5)&&(0<=pos2)&&(pos2<=5))

    {

      dir=1;

    }

    else

      if((6<=pos1)&&(pos1<=11)&&(6<=pos2)&&(pos2<=11))

      {

        dir=2;

      }

      else

        if((12<=pos1)&&(pos1<=17)&&(12<=pos2)&&(pos2<=17))

        {

          dir=3;

        }

        else

          if((0<=pos1)&&(pos1<=5)&&(6<=pos2)&&(pos2<=11)&&((5-pos1)<(pos2-5)))

          {

            dir=2;

          }

          else

            if((0<=pos1)&&(pos1<=5)&&(6<=pos2)&&(pos2<=11)&&((5-pos1)>(pos2-5)))

            {

              dir=1;

            }

            else

              if((5<=pos1)&&(pos1<=11)&&(12<=pos2)&&(pos2<=17)&&((11-pos1)>(pos2-11)))

              {

                dir=1;

              }

              else

                if((5<=pos1)&&(pos1<=11)&&(12<=pos2)&&(pos2<=17)&&((11-pos1)<(pos2-11)))

                {

                  dir=3;

                }

                else

              {

                dir=4;

              }

    return dir;

  }

//  else

//    stop();

}

int distance(int val)

{

  int dis;

  //dis=(2914 / (val + 5)) - 1;

  dis=2076/(val-11);

  //Serial.println(dis);

  return dis;

}

void forward()

{

  //  int dist2,dist3;

  //  dist2=analogRead(sensorpin1);

  //  while(obstacle>dist2)

  //  {

  //    stop();

  //    backward();

  //    right();

  //  }

  //  dist3=analogRead(sensorpin2);

  //  while(obstacle>dist3)

  //  {

  //    stop();

  //    backward();

  //    left();

  //  }

  int dist2;

  dist2=analogRead(sensorpin1);

  if (dist2>obstacle)

  {

    digitalWrite(STBY,HIGH);

 

    digitalWrite(AIN1,LOW);

    digitalWrite(AIN2,HIGH);

    digitalWrite(BIN1,LOW);

    digitalWrite(BIN2,HIGH);

 

    analogWrite(PWMA,200);

    analogWrite(PWMB,200);

    delay(100);

  }

  else

    backward();

}

 

void backward()

{

  digitalWrite(STBY,HIGH);

 

  digitalWrite(AIN1,HIGH);

  digitalWrite(AIN2,LOW);

  digitalWrite(BIN1,HIGH);

  digitalWrite(BIN2,LOW);

 

  analogWrite(PWMA,200);

  analogWrite(PWMB,200);

  delay(100);

}

 

void right()

{

  digitalWrite(STBY,HIGH);

 

  digitalWrite(AIN1,LOW);

  digitalWrite(AIN2,HIGH);

  digitalWrite(BIN1,HIGH);

  digitalWrite(BIN2,LOW);

 

  analogWrite(PWMA,200);

  analogWrite(PWMB,200);

  delay(100);

  stop();

}

 

void left()

 

{

  digitalWrite(STBY,HIGH);

 

  digitalWrite(AIN1,HIGH);

  digitalWrite(AIN2,LOW);

  digitalWrite(BIN1,LOW);

  digitalWrite(BIN2,HIGH);

 

  analogWrite(PWMA,200);

  analogWrite(PWMB,200);

  delay(100);

  stop();

}

/*void ultimatebackward()

 {

 digitalWrite(STBY,HIGH);

 

 //  digitalWrite(AIN1,HIGH);

 //  digitalWrite(AIN2,LOW);

 //  digitalWrite(BIN1,HIGH);

 //  digitalWrite(BIN2,LOW);

 //

 //  analogWrite(PWMA,200);

 //  analogWrite(PWMB,200);

 //  delay(250);

 

 digitalWrite(AIN1,HIGH);

 digitalWrite(AIN2,LOW);

 digitalWrite(BIN1,LOW);

 digitalWrite(BIN2,HIGH);

 analogWrite(PWMA,200);

 analogWrite(PWMB,200);

 delay(2000);

 

 } */

 

void stop()

{

  digitalWrite(STBY,LOW);

  delay(10);

 

}

 

5

A couple things.

Either use the built in copy as html function in the arudino IDE and then paste it properly in the text box, use something like hilite.me and paste the result properly in the text box, or use pastebin as I did. I can’t stand having to kill all of your extra whitespace to figure out what your code is doing. I probably spend 5 minutes just cleaning the code up to look at it.

Second, your problem currently seems to be a debug issue. Have your robot tell you what the value of d is so you can figure out which of the select/case statements it follows.