Hello there, I had been struggling with building a robot that follows along color line using two color sensors with 4 motors. Not sure it's software or hardware problem but the car is not moving at all. I give the supply of 12V with external adjustable power supply. Please, can someone help with this problem. Here is the code I'm using-
#define enA 10//Enable1 L298 Pin enA
#define in1 9 //Motor1 L298 Pin in1
#define in2 8 //Motor1 L298 Pin in1
#define in3 7 //Motor2 L298 Pin in1
#define in4 6 //Motor2 L298 Pin in1
#define enB 11//Enable2 L298 Pin enB
const int speed = 50;
//Color Sensor
const int s0 = 4; //Conection for Sensor
const int s1 = 5;
const int s2L = A0;
const int s3L = A1;
const int outL = A2;
int redL = 0; //Variablen for Left LEDs named
int greenL = 0;
int blueL = 0;
const int s2R = A3;
const int s3R = A4;
const int outR = A5;
int redR = 0; //Variablen for Right LEDs named
int greenR = 0;
int blueR = 0;
int sensor_left = 0;
int sensor_right = 0;
//int sensor_back = 0;
void setup()
{
Serial.begin(9600); //Serielle Kommunikation
//MOTOR PINS
pinMode(enA, OUTPUT); // declare as output for L298 Pin enA
pinMode(in1, OUTPUT); // declare as output for L298 Pin in1
pinMode(in2, OUTPUT); // declare as output for L298 Pin in2
pinMode(in3, OUTPUT); // declare as output for L298 Pin in3
pinMode(in4, OUTPUT); // declare as output for L298 Pin in4
pinMode(enB, OUTPUT); // declare as output for L298 Pin enB
//analogWrite(enA, 200); // Write The Duty Cycle 0 to 255 Enable Pin A for Motor1 Speed
//analogWrite(enB, 200); // Write The Duty Cycle 0 to 255 Enable Pin B for Motor2 Speed
//COLOR Sensor
pinMode(s0, OUTPUT);
pinMode(s1, OUTPUT);
digitalWrite(s0, HIGH);
digitalWrite(s1, HIGH);
pinMode(s2L, OUTPUT);
pinMode(s3L, OUTPUT);
pinMode(outL, INPUT);
pinMode(s2R, OUTPUT);
pinMode(s3R, OUTPUT);
pinMode(outR, INPUT);
}
void loop()
{
color();
Serial.print("LEft_");
Serial.print(redL);
Serial.print(",");
Serial.print(greenL);
Serial.print(",");
Serial.print(blueL);
Serial.print(",");
Serial.print("Right_");
Serial.print(redR);
Serial.print(",");
Serial.print(greenR);
Serial.print(",");
Serial.println(blueR);
delay(1000);
{ //detection
{ //Sensor Left
if (redL <= 13 && blueL >= 15 && greenL >= 15 && redL < blueL && redL < greenL)
{
sensor_left = 1;
}
else
{
sensor_left = 0;
}
}
{ //Sensor Right
if (redR <= 13 && blueR >= 15 && greenR >= 15 && redR < blueR && redR < greenR)
{
sensor_right = 1;
}
else
{
sensor_right = 0;
}
}
}
{ //Drive
if (sensor_left ==1 or sensor_right==1)
{
Found:
if (sensor_left == 1 && sensor_right == 1)
{//forward
forward();
delay(7);
stop();
Serial.print("Going forward ");
}
else if (sensor_left == 0 && sensor_right == 1)
{//Right Curve
turnRight();
delay(7);
stop();
Serial.print("Turning Right");
}
else if (sensor_left == 1 && sensor_right == 0)
{//Left Curve
turnLeft();
delay(7);
stop();
Serial.print("Turning Left");
}
}
else
{
Serial.println("Color detection ERROR!");
}
}
}
void color()
{
{ //Left Sensor
digitalWrite(s2L, LOW);
digitalWrite(s3L, LOW);
redL = pulseIn(outL, digitalRead(outL) == HIGH ? LOW : HIGH);
digitalWrite(s3L, HIGH);
blueL = pulseIn(outL, digitalRead(outL) == HIGH ? LOW : HIGH);
digitalWrite(s2L, HIGH);
greenL = pulseIn(outL, digitalRead(outL) == HIGH ? LOW : HIGH);
}
{ //Right Sensor
digitalWrite(s2R, LOW);
digitalWrite(s3R, LOW);
redR = pulseIn(outR, digitalRead(outR) == HIGH ? LOW : HIGH);
digitalWrite(s3R, HIGH);
blueR = pulseIn(outR, digitalRead(outR) == HIGH ? LOW : HIGH);
digitalWrite(s2R, HIGH);
greenR = pulseIn(outR, digitalRead(outR) == HIGH ? LOW : HIGH);
}
}
//MOTOR
void forward()
{
digitalWrite(in1, HIGH);
digitalWrite(in2, LOW);
analogWrite(enA, speed);
digitalWrite(in3, HIGH);
digitalWrite(in4, LOW);
analogWrite(enB, speed);
}
void backward()
{
digitalWrite(in1, LOW);
digitalWrite(in2, HIGH);
analogWrite(enA, speed);
digitalWrite(in3, LOW);
digitalWrite(in4, HIGH);
analogWrite(enB, speed);
}
void stop()
{
digitalWrite(in1, LOW);
digitalWrite(in2, LOW);
digitalWrite(in3, LOW);
digitalWrite(in4, LOW);
}
void turnLeft()
{
digitalWrite(in1, HIGH);
digitalWrite(in2, LOW);
analogWrite(enA, speed);
digitalWrite(in3, LOW);
digitalWrite(in4, HIGH);
analogWrite(enB, speed);
}
void turnRight()
{
digitalWrite(in1, LOW); //Left Motor backword Pin
digitalWrite(in2, HIGH); //Left Motor forword Pin
analogWrite(enA, speed);
digitalWrite(in3, HIGH);
digitalWrite(in4, LOW);
analogWrite(enB, speed);
}
