hey guys 
am having an issue with my grad project, am tasked with making a robot that moves in a square wave pattern like the first picture below in attachments ,while applying obstacle avoidance as well .
and in order to know how much distance did the little guy go i used IR encoders by counting the number of clicks the robot makes while moving and using that number in the following formula to get the distance
Distance traveled = (Encoder ticks / 360) * circumference
now the robot doesn't move at all, here is the code am using
typedef struct
{
int X;
int Y;
bool scanned;
bool blocked;
}cell;
int Echo1 = A2;
int Trig1 = A3;
int in1 = 4;
int in2 = 5;
int in3 = 6;
int in4 = 7;
int ENA = 8;
int ENB = 9;
int encoder_left = 2;
int encoder_right = 3;
int leftclick,rightclick=0;
float leftdis,rightdis=0;
float lt,rt=0.16328;
int i,j,k,l,o,p,q,r,s,t=0;
int m =3/0.165;
int n =2/0.157;
int z = m*n;
int Distance = 0;
//Distance traveled = (Encoder ticks / 360) * circumference
void calculateright(void)
{
if (digitalRead(encoder_right)==HIGH)
{
rightclick++;
}
rightdis=((float)rightclick/360)*(0.16328);
}
void calculateleft(void)
{
if (digitalRead(encoder_left)==HIGH)
{
leftclick++;
}
leftdis=((float)leftclick/360)*(0.16328);
}
int Distance_test()
{
digitalWrite(Trig1, LOW);
delayMicroseconds(2);
digitalWrite(Trig1, HIGH);
delayMicroseconds(20);
digitalWrite(Trig1, LOW);
float Fdistance = pulseIn(Echo1, HIGH);
delay(10);
Fdistance = Fdistance / 29 / 2;
Serial.println(Fdistance);
return (int)Fdistance;
}
void Enable(void)
{
digitalWrite(ENA, HIGH);
digitalWrite(ENB, HIGH);
}
void Forward(void)
{
while(leftdis<lt&&rightdis<rt)
{
calculateleft();
calculateright();
digitalWrite(in1, HIGH);
digitalWrite(in2, LOW);
digitalWrite(in3, LOW);
digitalWrite(in4, HIGH);
Serial.println("ROBOT_MOVING_FORWARD");
}
leftclick=0;
rightclick=0;
leftdis=0;
rightdis=0;
}
void Backward(void)
{
while(leftdis<lt&&rightdis<rt)
{
calculateleft();
calculateright();
digitalWrite(in1, LOW);
digitalWrite(in2, HIGH);
digitalWrite(in3, HIGH);
digitalWrite(in4, LOW);
Serial.println("ROBOT_MOVING_BACKWARD");
}
leftclick=0;
rightclick=0;
leftdis=0;
rightdis=0;
}
void Left(void)
{
while(leftdis<lt/2&&rightdis<rt/2)
{
calculateleft();
calculateright();
digitalWrite(in1, HIGH);
digitalWrite(in2, LOW);
digitalWrite(in3, HIGH);
digitalWrite(in4, LOW);
Serial.println("ROBOT_MOVING_LEFT");
}
leftclick=0;
rightclick=0;
leftdis=0;
rightdis=0;
}
void Right(void)
{
while(leftdis<lt/2&&rightdis<rt/2)
{
calculateleft();
calculateright();
digitalWrite(in1, LOW);
digitalWrite(in2, HIGH);
digitalWrite(in3, LOW);
digitalWrite(in4, HIGH);
Serial.println("ROBOT_MOVING_RIGHT");
}
leftclick=0;
rightclick=0;
leftdis=0;
rightdis=0;
}
void Stop(void)
{
digitalWrite(in1, LOW);
digitalWrite(in2, LOW);
digitalWrite(in3, LOW);
digitalWrite(in4, LOW);
Serial.println("ROBOT_STOP");
}
void Disable(void)
{
digitalWrite(ENA, LOW);
digitalWrite(ENB, LOW);
}
void setup()
{
Enable();
Stop();
Serial.begin(9600);
pinMode(in1,OUTPUT);
pinMode(in2,OUTPUT);
pinMode(in3,OUTPUT);
pinMode(in4,OUTPUT);
pinMode(ENA,OUTPUT);
pinMode(ENB,OUTPUT);
pinMode(Echo1, INPUT);
pinMode(Trig1, OUTPUT);
pinMode(encoder_left, INPUT);
pinMode(encoder_right, INPUT);
}
void loop()
{
cell c[m][n];
for( k=0; k<m; k++)
{
for( i=0; i<n; i++)
{
c[k][i].X =i;
c[k][i].Y =k;
Distance =Distance_test();
if(i*k == z)
{
Stop ();
}
if(i== n-1)
{
if(Distance>=10)
{
if(t%2==0)
{
Right ();
Forward ();
Right ();
c[k][i]. scanned =true;
c[k][i].blocked=false;
}
else
{
Left ();
Forward ();
Left ();
c[k][i]. scanned =true;
c[k][i].blocked=false;
}
}
else if (Distance<10)
{
if(t%2==0)
{
Right ();
Forward ();
Left ();
Right ();
Right ();
c[k][i]. scanned =false;
c[k][i]. blocked=true;
}
else
{
Left ();
Forward ();
Right ();
Left ();
Left ();
c[k][i]. scanned =false;
c[k][i]. blocked=true;
}
}
t++;
}
else
{
if(Distance>=10)
{
Forward ();
c[k][i]. scanned =true;
c[k][i].blocked=false;
}
else if(Distance<10)
{
Left ();
Forward ();
Right ();
Forward ();
Right ();
Forward ();
Left ();
c[k][i]. scanned =false;
c[k][i]. blocked=true;
}
}
}
}
Disable();
}
and for hardware am using the dagu DG012-SV tank frame which is equipped with two 5v dc motors
am using a single HC-SR04 for obstacle avoidance and 2 Encoder FC-03 one for each motor and a 11.1V-5.2A 3 cell Lipo battery with a dc-dc voltage converter that takes from 12-48v and produce 5vdc with 5A ,am using a L298 Dual Motor Driver Module 2A to drive the 2 motors
if anyone can point me in the right direction so i can fix the problem i'll be very thankfull
DG02S.pdf (111 KB)