Help with line follower code

[saba_rish91]

I built a line following robot with 5 analog IR sensors. It doesn't follow the line that well. Can anyone please check my code. I have used Arduino UNO development board to control the robot.

Code: [Select]

int s1=A0, s2=A1, s3=A2, s4=A3, s5=A4;
int v1,v2,v3,v4,v5;
int thres_val=350;
 
void setup()
{
 pinMode(13,OUTPUT); //H-bridge
 pinMode(12,OUTPUT); //H-bridge
 pinMode(8,OUTPUT); //H-bridge
 pinMode(7,OUTPUT); //H-bridge
}

void fwd(int a)
{
 digitalWrite(13,HIGH);
 digitalWrite(12,LOW);
 digitalWrite(8,HIGH);
 digitalWrite(7,LOW);
 delay(a);
}
 
void back(int a)
{
 digitalWrite(13,LOW);
 digitalWrite(12,HIGH);
 digitalWrite(8,LOW);
 digitalWrite(7,HIGH);
 delay(a);
}
 
void right(int a)
{
 digitalWrite(13,LOW);
 digitalWrite(12,LOW);
 digitalWrite(8,HIGH);
 digitalWrite(7,LOW);
 delay(a);
}
 
void left(int a)
{
 digitalWrite(13,HIGH);
 digitalWrite(12,LOW);
 digitalWrite(8,HIGH);
 digitalWrite(7,HIGH);
 delay(a);
}
 
void U_left(int a)
{
 digitalWrite(13,LOW);
 digitalWrite(12,HIGH);
 digitalWrite(8,HIGH);
 digitalWrite(7,LOW);
 delay(a);
}
 
void U_right(int a)
{
 digitalWrite(13,HIGH);
 digitalWrite(12,LOW);
 digitalWrite(8,LOW);
 digitalWrite(7,HIGH);
 delay(a);
}
 
void com_stop(int a)
{
 digitalWrite(13,LOW);
 digitalWrite(12,LOW);
 digitalWrite(8,LOW);
 digitalWrite(7,LOW);
 delay(a);
}

void sen_read()
{
 v1=analogRead(s1);
 v2=analogRead(s2);
 v3=analogRead(s3);
 v4=analogRead(s4);
 v5=analogRead(s5);
 delay(7);
}
 
void navigate()
{
  //sen_read();
 if((v1<thres_val)&&(v2<thres_val)&&(v3>thres_val)&&(v4<thres_val)&&(v5<thres_val))
 {
   fwd(3);
 }
 else if((v1<thres_val)&&(v2>thres_val)&&(v3>thres_val)&&(v4<thres_val)&&(v5<thres_val))
 {
   left(1);
 }
 else if((v1<thres_val)&&(v2>thres_val)&&(v3<thres_val)&&(v4<thres_val)&&(v5<thres_val))
 {
   left(1);
 }
 else if((v1>thres_val)&&(v2>thres_val)&&(v3<thres_val)&&(v4<thres_val)&&(v5<thres_val))
 {
   left(1);
 }
 else if((v1>thres_val)&&(v2<thres_val)&&(v3<thres_val)&&(v4<thres_val)&&(v5<thres_val))
 {
   left(1);
 }
 else if((v1<thres_val)&&(v2<thres_val)&&(v3>thres_val)&&(v4>thres_val)&&(v5<thres_val))
 {
   right(1);
 }
 else if((v1<thres_val)&&(v2<thres_val)&&(v3<thres_val)&&(v4>thres_val)&&(v5<thres_val))
 {
   right(1);
 }
 else if((v1<thres_val)&&(v2<thres_val)&&(v3<thres_val)&&(v4>thres_val)&&(v5>thres_val))
 {
   right(1);
 }
 else if((v1<thres_val)&&(v2<thres_val)&&(v3<thres_val)&&(v4<thres_val)&&(v5>thres_val))
 {
   right(1);
 }
 else if((v1>thres_val)&&(v2>thres_val)&&(v3>thres_val)&&(v4<thres_val)&&(v5<thres_val))
 {
   U_left(250);
 }
 else if((v1<thres_val)&&(v2<thres_val)&&(v3>thres_val)&&(v4>thres_val)&&(v5>thres_val))
 {
   U_right(250);
 }
 else if((v1>thres_val)&&(v2>thres_val)&&(v3>thres_val)&&(v4>thres_val)&&(v5<thres_val))
 {
   U_left(250);
 }
 else if((v1<thres_val)&&(v2>thres_val)&&(v3>thres_val)&&(v4>thres_val)&&(v5>thres_val))
 {
   U_right(250);
 }
 else if((v1<thres_val)&&(v2>thres_val)&&(v3>thres_val)&&(v4>thres_val)&&(v5<thres_val))
 {
   fwd(1);
 }
 else if((v1>thres_val)&&(v2>thres_val)&&(v3>thres_val)&&(v4>thres_val)&&(v5>thres_val))
 {
   U_left(250);
 }
 else if((v1<thres_val)&&(v2<thres_val)&&(v3<thres_val)&&(v4<thres_val)&&(v5<thres_val))
 {
   back(1);
 }
 else
 {
   fwd(3);
 }
}

void loop()
{
  sen_read();
  navigate();
}

Thanks in advance.

[saba_rish91]

Also I tried to implement PID algorithm. This is correct?

Code: [Select]

 
  prop=kp*error;
  integral=ki*(error+lst_error);
  dervi=kd*(error-lt_error);
  lt_error=error;
  lst_error+=error;
  P=prop+integral+dervi;

Where lst_error is the summation of all the previous errors and lt_error is the previous error.
If P is greater than zero, then I increase the speed of right motor and decrease the speed of left motor and if P is lesser than zero then vice verse.

[agold]

Your first code seems to be fine from the quick read through it I did, one line which seems a bit weird is

Code: [Select]

else if((v1>thres_val)&&(v2>thres_val)&&(v3>thres_val)&&(v4>thres_val)&&(v5>thres_val))
 {
   U_left(250);
 }
I think that's making it turn left when it sees the line everywhere, forward is probably a better option... though there are very few cases where that would happen anyway.

Also, in terms of readability, I'd recommend in sens_read(), assign boolean variables like sensor1_white to (v1 > thres_value), which will store whether or not sensor1 can see the line (which I'm assuming is white). This will mean your if statements can be written more like

Code: [Select]

if (sensor1_white && sensor2_white && sensor3_black && sensor4_black && sensor5_black)
...

For the PID contraoller, you have the right idea, a couple of things I came across when writing PIDs for line following:
* you can set ki to zero (i.e. ignore the integral component) which often gives a better result
* if you do use the integral, it's worth testing it with resetting it to 0 once the error again becomes 0
* watch out for the range of your numbers and whether they'll overflow the variables (especially lst_error). The range for an int in arduino is -2^16 ... 2^16-1, and -2^32 ... 2^32-1 for a long (you'll need them signed as error can go below 0).
* I used a 'base speed' which I'd set to around 75% of max speed then have motora = base+P motorb = base-P (or reversed if P < 0)

I assume you've come across this but in case not, assign a number to each sensor (e.g. sensor 1 = 10, sensor2 = 20, etc) and then find the average of IDs of the sensors that are sensing the line (e.g. if s2 and s3 could see the line, your position would be (20+30)/2 = 25), subtract you desired position to get the error.

Finally, be prepared to spend a while tuning the constants kp,ki&kd, it *does* take a while.

Adrian