I am building a line following robot that has 5 ir sensors, everything seems to work fine with the code until my robot gets to a turn, instead of following the code and turning it just stops. I believe this has something to do with the code but I can't seem to find the problem. If any of you could find a problem with my code thank you in advance.
#include<Servo.h>
Servo motorL;
Servo motorR;
Servo motorP;
const int rsensor = 7;
const int midsensor = 8;
const int lsensor = 12;
const int flsensor = 10;
const int frsensor = 5;
const int rled = A2;
const int gled = A1;
void setup() {
// put your setup code here, to run once:
motorL.attach(4);
motorR.attach(2);
motorP.attach(3);
pinMode(rsensor, INPUT);
pinMode(midsensor, INPUT);
pinMode(lsensor, INPUT);
pinMode(frsensor, INPUT);
pinMode(flsensor, INPUT);
Serial.begin(9600);
}
void loop() {
int turns = 0;
// put your main code here, to run repeatedly:
while (!digitalRead(midsensor) && !digitalRead(rsensor) && !digitalRead(lsensor) && !digitalRead(flsensor) && !digitalRead(frsensor))
{
both_wheel_stop();
}
if (digitalRead(midsensor) && digitalRead(lsensor) && digitalRead(rsensor) && digitalRead(frsensor) && digitalRead(flsensor))
{
turnLeft();
turns++;
}
else if (digitalRead(midsensor) && digitalRead(lsensor) && digitalRead(flsensor))
{
turnLeft();
turns++;
}
else if (digitalRead(midsensor) && digitalRead(rsensor) && digitalRead(frsensor))
{
if (turns == 4 && digitalRead(rsensor) && digitalRead(frsensor))
{
straight_fast();
}
else
{
turnRight();
turns++;
}
}
else if (!digitalRead(midsensor) && !digitalRead(rsensor))
{
while (!digitalRead(midsensor))
{
adjustLeft();
delay(300);
}
}
else if (!digitalRead(midsensor) && !digitalRead(lsensor))
{
while (!digitalRead(midsensor))
{
adjustRight();
delay(300);
}
}
else if(turns == 6)
{
for(int k = 0; k <= 2; k++)
{
straight_fast();
}
motorP.detach();
motorL.detach();
motorR.detach();
}
else
{
straight_slow();
}
}
void straight_slow()
{
motorL.write(100); // Slow speed straight
motorR.write(80);
}
void straight_fast()
{
motorL.write(180); // full speed straight
motorR.write(0);
}
void both_wheel_stop()
{
motorL.write(90); // stop both servo
motorR.write(90);
}
void adjustLeft()
{
motorL.write(100); // Left wheel stop
motorR.write(90);
delay(50);
}
void adjustRight()
{
motorL.write(90); // right wheel stop
motorR.write(80);
delay(50);
}
void turnRight()
{
while (!digitalRead(midsensor))
{
motorL.write(100);
motorR.write(80);
}
}
void turnLeft()
{
while (!digitalRead(midsensor))
{
motorL.write(80);
motorR.write(100);
}
}
5_sensor_kashika.ino (2.43 KB)