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

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*=distance(analogRead(sensorpin3));*

  • delay(100);*
    _ if(dist*<20)_
    _
    {_
    _ dist=0;
    }
    else*
    dist*=1;
    }
    for(pos=180,i=17; pos>=0; pos-=10,i–)
    {
    myservo.write(pos);*_

_ dist*=distance(analogRead(sensorpin3));
delay(100);
if(dist<20)
{
dist=0;
}
else*

dist*=1;
}
}
int direction()
{*_

* int count1,count2,newcount,pos1,pos2;*
* for(int i=1;i<=17;i++)*
* {*
_ if(dist*==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);*

* }*

Now, without italics, please.

i have attached the code …

mapping.ino (4.74 KB)

#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);
   
   }

Please tel me where i am going wrong or whether my logic s correct or not???

Why don’t you tell us if the code works, or not?

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

This, of course, is rubbish. Comparing the value of true (if pos1 is positive) or false (if pos1 is negative) to 5 is pointless.

int dist[17];
for(pos=0, i=0; pos<=180; pos+=10,i++)
   {
     myservo.write(pos);
    
    dist[i]=distance(analogRead(sensorpin3));