Society of Robots - Robot Forum
Software => Software => Topic started by: saba_rish91 on January 13, 2013, 11:26:15 AM
-
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.
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.
-
Also I tried to implement PID algorithm. This is correct?
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.
-
Your first code seems to be fine from the quick read through it I did, one line which seems a bit weird is
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
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