Hello,
I’ve bought a line follower with 3 sensors and I installed on my rover, but I’ve got a problem with the code.
As usual I used the code that has been done (on this site), but the robot can’t achieve the curved line (like 90 degrees) for this reason I’ve changed the code like:
[code]#define NUM_SENSOR 3 // number of sensors
#define kp 50 // constant for PDI algorithm
int rsensor = 2; // Left Sensor on Analog Pin 2
int lsensor = 1; // Right Sensor on Analog Pin 1
int msensor = 0; // Middle Sensor on Analog Pin 0
int E1 = 6; //M1 Speed Control
int E2 = 5; //M2 Speed Control
int M1 = 8; //M1 Direction Control
int M2 = 7; //M2 Direction Control
const int whitelevl = 600; // reading level is white if <600
const int blacklevl = 850; // reading level is black if >850
int i;
void setup()
{
Serial.begin(9600);
for (int i = 0; i < 100; i++) // calibrate for sometime by sliding the sensors across the line
if ( i < 25 || i >= 75 ) // turn to the left and right to expose the sensors to the brightest and darkest readings that may be encountered
turnright();
else
turnleft();
}
void loop()
{
int LF[NUM_SENSOR];
int error = 0;
LF[0] = readQD(lsensor);
LF[1] = readQD(msensor);
LF[2] = readQD(rsensor);
// Case 1 : Left sensor is reading white, middle sensor is reading black and right sensor is reading white. Go forward!
if((LF[0] <= whitelevl)&&(LF[1]>= blacklevl) && (LF[2]>=whitelevl)) //wBw
{
while(!true){
error = 0;
wait();
break;
}
}
// Case 2 : Left and middle sensor are reading white, right sensor is reading black. Turn right!
else if ((LF[0] <= whitelevl) && (LF[1] <= whitelevl) && (LF[2] >= blacklevl)) //wwB
{
while(true){
error = 3;
wait();
if(!((LF[0] <= whitelevl)&&(LF[1]>= blacklevl) && (LF[2]>=whitelevl))){
for (int i = 0; i < 100; i++) // calibrate for sometime by sliding the sensors across the line
if ( i < 25 || i >= 75 ) // turn to the left and right to expose the sensors to the brightest and darkest readings that may be encountered
turnright();
else
turnleft();
}
break;
}
}
// Case 3 : Left sensor is reading white, middle sensor and right sensor are reading black. Turn right!
else if ((LF[0] <= whitelevl) && (LF[1] >= blacklevl) && (LF[2] >= blacklevl))
{ //wBB
while(true){
error = 3;
wait();
if(!((LF[0] <= whitelevl)&&(LF[1]>= blacklevl) && (LF[2]>=whitelevl))){
for (int i = 0; i < 100; i++) // calibrate for sometime by sliding the sensors across the line
if ( i < 25 || i >= 75 ) // turn to the left and right to expose the sensors to the brightest and darkest readings that may be encountered
turnright();
else
turnleft();
}
break;
}
}
// Case 4 : Left sensor is reading black, middle sensor and right sensor are reading white. Turn left!
else if ((LF[0] >= blacklevl) && (LF[1] <= whitelevl) && (LF[2] <= whitelevl)) //Bww
{
while(true){
error = 1;
wait();
if(!((LF[0] <= whitelevl)&&(LF[1]>= blacklevl) && (LF[2]>=whitelevl))){
for (int i = 0; i < 100; i++) // calibrate for sometime by sliding the sensors across the line
if ( i < 25 || i >= 75 ) // turn to the left and right to expose the sensors to the brightest and darkest readings that may be encountered
turnright();
else
turnleft();
}
break;
}
}
// Case 5 : Left sensor and middle sensor are reading black and right sensor is reading white. Turn left!
else if ((LF[0] >= blacklevl) && (LF[1] >= blacklevl) && (LF[2] <= whitelevl)) //BBw
{
while(true){
error = 1;
wait();
if(!((LF[0] <= whitelevl)&&(LF[1]>= blacklevl) && (LF[2]>=whitelevl))){
for (int i = 0; i < 100; i++) // calibrate for sometime by sliding the sensors across the line
if ( i < 25 || i >= 75 ) // turn to the left and right to expose the sensors to the brightest and darkest readings that may be encountered
turnright();
else
turnleft();
}
break;
}
}
switch(error){
case 0:
goforward();
break;
case 1:
turnleft();
break;
case 3:
turnright();
break;
default:
goforward(); //temp
break;
}
}
/////////////// Go Forward Routine ///////////////////
void goforward(){
analogWrite (E2,255);
digitalWrite(M1,LOW);
analogWrite (E1,255);
digitalWrite(M2,LOW);
delay(20);
}
////////////////////////////////////////////////////////
///////////////// Turn Right Routine ///////////////////
void turnright(){
analogWrite (E2,210-kp);
digitalWrite(M1,LOW);
analogWrite (E1,210);
digitalWrite(M2,HIGH);
delay(40);
}
////////////////////////////////////////////////////////
///////////////// Turn Left Routine ///////////////////
void turnleft(){
analogWrite (E2,210);
digitalWrite(M1,HIGH);
analogWrite (E1,210-kp);
digitalWrite(M2,LOW);
delay(40);
}
void wait(){
delay(100);
}
//////////////////////////////////////////////////////
////////////////////// Read Sensor Routine //////////
int readQD(int x){
int val = analogRead(x);
return val;
}
//////////////////////////////////////////////////////
[/code]
Could you give me a feedback on the code?
Thank you in advance!
Best regards,
Alberto