I coudnt post the code in the post above so,
here is the code:
#define DUR_GIRO 100//duration of movinemto counterclockwise and clockwise
#define MOV_I 200//duration of movinemto back
#define MOV_A 10//duration of movinemto forward
#define trigger_sx 2
#define echo_sx 4
#define trigger_an 7
#define echo_an 8
#define trigger_dx 9
#define echo_dx 12
#include <Servo.h>
Servo myservo;
byte limit=12;//the reading value of ultrasound in cm
byte byte_S,byte_A,byte_D;
//if < = 20 and as if it were the first
//if > 20 and as 0
byte value_lecture;
byte VEL_AVAN=200;
byte VEL_IND=80;
byte VEL_LATERAL=90;
byte cc=5;
int C=0;//used to do in sensor_A and sensor_O
long duration;
long lecture_sx;
long lecture_an;
long lecture_dx;
unsigned long time=0;//variable timer
//---code v2.5 this is my prototype...
void fermo(void);
void scan (void);
void ind(unsigned char);
void avan(unsigned char);
void sensor_O(void);
void sensor_A(void);
void moviment(void);
void script(void);
void setup(void)
{
myservo.attach(10);
pinMode(5,OUTPUT);
pinMode(6,OUTPUT);
pinMode(11,OUTPUT);
pinMode(3,OUTPUT);
pinMode( trigger_sx, OUTPUT );
pinMode( echo_sx, INPUT );
pinMode( trigger_dx, OUTPUT );
pinMode( echo_dx, INPUT );
pinMode( trigger_an, OUTPUT );
pinMode( echo_an, INPUT );
Serial.begin(9600);
delay(2000);
}
void loop(void)
{
time = millis();
if(time>=600000) //after 10 minutes it stops automatically
{
fermo();
return;
}
scan();
moviment();
}
void fermo(void)
{
digitalWrite(3,LOW);
digitalWrite(5,LOW);
digitalWrite(6,LOW);
digitalWrite(11,LOW);
}
void ind(unsigned char ind)
{
do
{
C++;
digitalWrite(3,LOW);
analogWrite(5,VEL_IND);
analogWrite(6,VEL_IND);
digitalWrite(11,LOW);
delay(1);
}
while(C<ind);
C=0;
return;
}
void avan(unsigned char avan)
{
do
{
C++;
analogWrite(3,VEL_AVAN);
digitalWrite(5,LOW);
digitalWrite(6,LOW);
analogWrite(11,VEL_AVAN);
delay(1);
}
while(C<avan);
C=0;
return;
}
void sensor_O(void)
{
do
{
C++;
digitalWrite(3,LOW);
analogWrite(5,VEL_LATERAL);
digitalWrite(6,LOW);
analogWrite(11,VEL_LATERAL);
delay(1);
}
while(C<DUR_GIRO);
C=0;
return;
}
void sensor_A(void)
{
do
{
C++;
analogWrite(3,VEL_LATERAL);
digitalWrite(5,LOW);
analogWrite(6,VEL_LATERAL);
digitalWrite(11,LOW);
delay(1);
}
while(C<DUR_GIRO);
C=0;
return;
}
void script(void)
{
Serial.print("Sx=");
Serial.print(lecture_sx);
Serial.print(" AN=");
Serial.print(lecture_an);
Serial.print(" Dx=");
Serial.print(lecture_dx);
Serial.print(" VL=");
Serial.println(value_lecture);
delay(100);
}
void scan(void)
{
//---READING SENSOR LEFT
digitalWrite( trigger_sx, LOW );
digitalWrite( trigger_sx, HIGH );
for(int uu=0;uu<=11;uu++)//look 15 micro seconds
{
delayMicroseconds( 1 );
}
digitalWrite( trigger_sx, LOW );
duration = pulseIn( echo_sx, HIGH );
lecture_sx = 0.02614 * duration / 2;//It is divided by 2 because the time proceeds roundtrip
if(lecture_sx>=200) lecture_sx=200;//if the reading exceeds 2 M expr is evaluated always set the maximum value that is 2 M
else lecture_sx=lecture_sx;//if the reading does not exceed 2 M then the reading remains unchanged
if(lecture_sx<=limit) byte_S=1;
else byte_S=0;
delayMicroseconds( cc );
duration=0;
//---READING SENSOR FRONT
digitalWrite( trigger_an, LOW );
digitalWrite( trigger_an, HIGH );
for(int uu=0;uu<=11;uu++)//look 15 micro seconds
{
delayMicroseconds( 1 );
}
digitalWrite( trigger_an, LOW );
duration = pulseIn( echo_an, HIGH );
lecture_an = 0.02614 * duration / 2;//It is divided by 2 because the time proceeds roundtrip
if(lecture_an>=200) lecture_an=200;//if the reading exceeds 2 M expr is evaluated always set the maximum value that is 2 M
else lecture_an=lecture_an;//if the reading does not exceed 2 M then the reading remains unchanged
if(lecture_an<=limit) byte_A=2;
else byte_A=0;
delayMicroseconds( cc );
duration=0;
//---READING SENSOR RIGHT
digitalWrite( trigger_dx, LOW );
digitalWrite( trigger_dx, HIGH );
for(int uu=0;uu<=11;uu++)//look 15 micro seconds
{
delayMicroseconds( 1 );
}
digitalWrite( trigger_dx, LOW );
duration = pulseIn( echo_dx, HIGH );
lecture_dx = 0.02614 * duration / 2;//It is divided by 2 because the time proceeds roundtrip
if(lecture_dx>=200) lecture_dx=200;//if the reading exceeds 2 M expr is evaluated always set the maximum value that is 2 M
else lecture_dx=lecture_dx;//if the reading does not exceed 2 M then the reading remains unchanged
if(lecture_dx<=limit) byte_D=4;
else byte_D=0;
delayMicroseconds( cc );
duration=0;
value_lecture=byte_S+byte_A+byte_D;//It returns the value of reading in binary
byte_S=0;
byte_A=0;
byte_D=0;
byte time1,time2,time3=0;
{
//---READING SENSOR LEFT
digitalWrite( trigger_sx, LOW );
digitalWrite( trigger_an, LOW );
digitalWrite( trigger_dx, LOW );
digitalWrite( trigger_sx, HIGH );
digitalWrite( trigger_an, HIGH );
digitalWrite( trigger_dx, HIGH );
for(int uu=0;uu<=20;uu++)//look 15 micro seconds
{
delayMicroseconds( 1 );
}
digitalWrite( trigger_sx, LOW );
digitalWrite( trigger_an, LOW );
digitalWrite( trigger_dx, LOW );
time1 = pulseIn( echo_sx, HIGH );
delayMicroseconds( cc );
time2 = pulseIn( echo_an, HIGH );
delayMicroseconds( cc );
time3 = pulseIn( echo_dx, HIGH );
delayMicroseconds( cc );
lecture_sx = 0.02614 * time1 / 2;//It is divided by 2 because the time proceeds roundtrip
lecture_an = 0.02614 * time2 / 2;//be divided by 2 because the time proceeds is forward and ritornodelayMicroseconds ( 5 ) ;
lecture_dx = 0.02614 * time3 / 2;//It is divided by 2 because the time proceeds roundtrip
if(lecture_sx<=limit) byte_S=1;
else byte_S=0;
if(lecture_an<=limit) byte_A=2;
else byte_A=0;
if(lecture_dx<=limit) byte_D=4;
else byte_D=0;
value_lecture=byte_S+byte_A+byte_D;
byte_S=0;
byte_A=0;
byte_D=0;
}
}
// Here is the moviment sector is divided in two parts
void moviment(void)
{
if((lecture_an<=15)&&(lecture_sx<=15)&&(lecture_dx<=15)) //if you are in a dead end goes back
{
for(int cacca=0;cacca<=10;cacca++)//It goes back for 1 second
{
ind();
delay(100);
}
}
else
{
if(lecture_an<=20) //if it has an obstacle in front controls whether to turn left or right
{
if(lecture_dx>=lecture_sx) sensor_O();//if I have obstacles in front and to the right you move anticlockwise
if(lecture_dx<lecture_sx) sensor_A();//if I have obstacles in front and to the right you move anticlockwise
}
else
{
if(lecture_dx<=15) sensor_A();
if(lecture_sx<=15) sensor_O();
else
{
avan();
}
}
}
if((lecture_an<=20)&&(lecture_dx<=20)) sensor_A();//if I have obstacles in front and to the right you move anticlockwise
else
{
ind();
}
if((lecture_an<=20)&&(lecture_sx<=20)) sensor_O();//if I have obstacles in front and to the right you move anticlockwise
else
{
ind();
}
}
void moviment_2(void)
{
switch(value_lecture)
{
case 0://if there are no obstacles
avan(MOV_A);
break;
case 1://if there is an obstacle left turn right
sensor_O();
break;
case 2://if there is an obstacle only to go ahead or to the right or left depends where or more space
if(lecture_dx>=lecture_sx) sensor_O();//if I have obstacles in front and to the right you move clockwise
if(lecture_dx<lecture_sx) sensor_A();//if I have obstacles in front and to the right you move anticlockwise
else ind(MOV_I);
break;
case 3://if there is an obstacle left front and I go right
ind(MOV_I);
sensor_O();
break;
case 4://if there is an obstacle to the right , turn left
sensor_A();
break;
case 5://
avan(MOV_A);
break;
case 6://if there is an obstacle ahead and go right to the left
ind(MOV_I);
sensor_A();
break;
case 7://if I did not go back outlets
ind(MOV_I);
break;
case 8://if there is an obstacle left turn right
default:
break;
}
}