car-robot-2-color-sensor-3-colors

#define S0 2
#define S1 3
#define S2 4
#define S3 A0
#define sensorOut A1
#define sensorOut2 11
#define S22 12
#define S23 13


int frequency = 0;
int color = 0;
int frequency2 = 0;
int color2 = 0;

#define BSW A2
#define RSW A3
#define GSW A4



#define MOTOR_SPEED 180 //180   0-255

//Right motor
int enableRightMotor = 6;
int rightMotorPin1 = 7;
int rightMotorPin2 = 8;

//Left motor
int enableLeftMotor = 5;
int leftMotorPin1 = 9;
int leftMotorPin2 = 10;

void setup()
{
  Serial.begin(9600);
  pinMode(S0 , OUTPUT);
  pinMode(S1 , OUTPUT);
  pinMode(S2 , OUTPUT);
  pinMode(S3 , OUTPUT);
  pinMode(S22 , OUTPUT);
  pinMode(S23 , OUTPUT);
  pinMode(sensorOut , INPUT);
  pinMode(sensorOut2 , INPUT);
  //Setting frequency-scaling
  digitalWrite(S0, HIGH);
  digitalWrite(S1, LOW);

  pinMode(BSW, INPUT);
  pinMode(RSW, INPUT);
  pinMode(GSW, INPUT);

  //The problem with TT gear motors is that, at very low pwm value it does not even rotate.
  //If we increase the PWM value then it rotates faster and our robot is not controlled in that speed and goes out of line.
  //For that we need to increase the frequency of analogWrite.
  //Below line is important to change the frequency of PWM signal on pin D5 and D6
  //Because of this, motor runs in controlled manner (lower speed) at high PWM value.
  //This sets frequecny as 7812.5 hz.
  TCCR0B = TCCR0B & B11111000 | B00000010 ;

  // put your setup code here, to run once:
  pinMode(enableRightMotor, OUTPUT);
  pinMode(rightMotorPin1, OUTPUT);
  pinMode(rightMotorPin2, OUTPUT);

  pinMode(enableLeftMotor, OUTPUT);
  pinMode(leftMotorPin1, OUTPUT);
  pinMode(leftMotorPin2, OUTPUT);

  rotateMotor(0, 0);
}


void loop()
{

  color  = readColor();// 0  1  2  3
  color2 = readColor2();
  delay(10);
  // 1 = RED
  // 2 = BLACK
  // 3 = GREEN
  int BSW_status = digitalRead(BSW);
  int RSW_status = digitalRead(RSW);
  int GSW_status = digitalRead(GSW);
  Serial.print(BSW_status);
  Serial.print(RSW_status);
  Serial.println(GSW_status);


  if (BSW_status == 1 && RSW_status == 0 && GSW_status == 0) {
    //Follow BLUE line
    // 2 = BLACK
    if (color == 2 && color2 == 2) {
      rotateMotor(MOTOR_SPEED, MOTOR_SPEED);//FRONT
    }
    else if (color == 2 && color2 != 2) {
      rotateMotor(MOTOR_SPEED, -MOTOR_SPEED);//LEFT
    }
    else if (color != 2 && color2 == 2) {
      rotateMotor(-MOTOR_SPEED, MOTOR_SPEED);//RIGHT
    }
    else if (color != 2 && color2 != 2) {
      rotateMotor(0, 0);//STOP
    }
  }
  else if (BSW_status == 0 && RSW_status == 1 && GSW_status == 0) {
    //Follow RED line
    // 1 = RED
    if (color == 1 && color2 == 1) {
      rotateMotor(MOTOR_SPEED, MOTOR_SPEED);//FRONT
    }
    else if (color == 1 && color2 != 1) {
      rotateMotor(MOTOR_SPEED, -MOTOR_SPEED);//LEFT
    }
    else if (color != 1 && color2 == 1) {
      rotateMotor(-MOTOR_SPEED, MOTOR_SPEED);//RIGHT
    }
    else if (color != 1 && color2 != 1) {
      rotateMotor(0, 0);//STOP
    }
  }

  else if (BSW_status == 0 && RSW_status == 0 && GSW_status == 1) {
    //Follow GREEN line
    // 3 = GREEN
    if (color == 3 && color2 == 3) {
      rotateMotor(MOTOR_SPEED, MOTOR_SPEED);//FRONT
    }
    else if (color == 3 && color2 != 3) {
      rotateMotor(MOTOR_SPEED, -MOTOR_SPEED);//LEFT
    }
    else if (color != 3 && color2 == 3) {
      rotateMotor(-MOTOR_SPEED, MOTOR_SPEED);//RIGHT
    }
    else if (color != 3 && color2 != 3) {
      rotateMotor(0, 0);//STOP
    }
  }

  else {
    rotateMotor(0, 0);
  }


}//end of LOOP


void rotateMotor(int rightMotorSpeed, int leftMotorSpeed)
{

  if (rightMotorSpeed < 0)
  {
    digitalWrite(rightMotorPin1, LOW);
    digitalWrite(rightMotorPin2, HIGH);
  }
  else if (rightMotorSpeed > 0)
  {
    digitalWrite(rightMotorPin1, HIGH);
    digitalWrite(rightMotorPin2, LOW);
  }
  else
  {
    digitalWrite(rightMotorPin1, LOW);
    digitalWrite(rightMotorPin2, LOW);
  }

  if (leftMotorSpeed < 0)
  {
    digitalWrite(leftMotorPin1, LOW);
    digitalWrite(leftMotorPin2, HIGH);
  }
  else if (leftMotorSpeed > 0)
  {
    digitalWrite(leftMotorPin1, HIGH);
    digitalWrite(leftMotorPin2, LOW);
  }
  else
  {
    digitalWrite(leftMotorPin1, LOW);
    digitalWrite(leftMotorPin2, LOW);
  }
  analogWrite(enableRightMotor, abs(rightMotorSpeed));
  analogWrite(enableLeftMotor, abs(leftMotorSpeed));
}



//Read-Color [1] Function

int readColor() {

  ////////////////////////////////////////////////////////

  //Setting red filtered photodiodes to be read

  digitalWrite(S2, LOW);
  digitalWrite(S3, LOW);


  //Reading the output frequency
  frequency = pulseIn(sensorOut, LOW);
  int R = frequency;
  //Printing the value on the serial monitor
  Serial.print("R= ");  //printing name
  Serial.print(frequency);  //printing RED color frequency
  Serial.print("  ");
  delay(50);

  ////////////////////////////////////////////////////////

  //Setting Green filtered photodiodes to be read

  digitalWrite(S2, HIGH);
  digitalWrite(S3, HIGH);

  //Reading the output frequency
  frequency = pulseIn(sensorOut, LOW);
  int G = frequency;

  //Printing the value on the serial monitor
  Serial.print("G= ");  //printing name
  Serial.print(frequency);  //printing RED color frequency
  Serial.print("  ");
  delay(50);

  ////////////////////////////////////////////////////////

  //Setting Blue filtered photodiodes to be read

  digitalWrite(S2, LOW);
  digitalWrite(S3, HIGH);

  //Reading the output frequency
  frequency = pulseIn(sensorOut, LOW);
  int B = frequency;

  //Printing the value on the serial monitor
  Serial.print("B= ");  //printing name
  Serial.print(frequency);  //printing RED color frequency
  Serial.println("  ");
  delay(50);

  ////////////////////////////////////////////////////////

  if (R < 57 && R > 0 && G < 110 && G > 40 && B < 80 && B > 25  ) {
    color = 1; // Red ✔[18-2-2022]
    Serial.println("\t\t\tRED detected!");
  }

  else if (R < 111 && R > 25 && G < 74 && G > 20 && B < 70 && B > 20  ) {
    color = 3; // Green ✔[19-2-2022]
    Serial.println("\t\t\tGREEN detected!");
  }
  else if (R < 140 && R > 20 && G < 150 && G > 30 && B < 120 && B > 16  ) {
    color = 2; // Black ✔[18-2-2022]
    Serial.println("\t\t\tBLACK detected!");
  }
  else {
    color = 0;
  }
  return color;
}

///////////////////////////////////////////////////////////////////////////////////////////////

//Read-Color [2] Function

int readColor2() {

  ////////////////////////////////////////////////////////

  //Setting red filtered photodiodes to be read

  digitalWrite(S22, LOW);
  digitalWrite(S23, LOW);


  //Reading the output frequency
  frequency2 = pulseIn(sensorOut2, LOW);
  int R2 = frequency2;
  //Printing the value on the serial monitor
  Serial.print("R2= ");  //printing name
  Serial.print(frequency2);  //printing RED color frequency
  Serial.print("  ");
  delay(50);

  ////////////////////////////////////////////////////////

  //Setting Green filtered photodiodes to be read

  digitalWrite(S22, HIGH);
  digitalWrite(S23, HIGH);

  //Reading the output frequency
  frequency2 = pulseIn(sensorOut2, LOW);
  int G2 = frequency2;

  //Printing the value on the serial monitor
  Serial.print("G2= ");  //printing name
  Serial.print(frequency2);  //printing RED color frequency
  Serial.print("  ");
  delay(50);

  ////////////////////////////////////////////////////////

  //Setting Blue filtered photodiodes to be read

  digitalWrite(S22, LOW);
  digitalWrite(S23, HIGH);

  //Reading the output frequency
  frequency2 = pulseIn(sensorOut2, LOW);
  int B2 = frequency2;

  //Printing the value on the serial monitor
  Serial.print("B2= ");  //printing name
  Serial.print(frequency2);  //printing RED color frequency
  Serial.println("  ");
  delay(50);

  ////////////////////////////////////////////////////////

  if (R2 < 57 && R2 > 10 && G2 < 130 && G2 > 40 && B2 < 89 && B2 > 30  ) {
    color2 = 1; // Red ✔[19-2-2022]
    Serial.println("\t\t\tRED detected! 2");
  }
  else if (R2 < 150 && R2 > 75 && G2 < 175 && G2 > 90 && B2 < 155 && B2 > 80  ) {
    color2 = 2; // Black ✔✔✔[14-2-2022]
    Serial.println("\t\t\tBLUE detected! 2");
  }
  else if (R2 < 130 && R2 > 30 && G2 < 79 && G2 > 20 && B2 < 81 && B2 > 25  ) {
    color2 = 3; // Green ✔[19-2-2022]
    Serial.println("\t\t\tGREEN detected! 2");
  }
  else {
    color2 = 0;
  }
  return color2;
}
///////////////////////////////////////////////////////////////////////////////////////////////

/*
  void followLine() {

  //If none of the sensors detects black line, then go straight
  if (rightIRSensorValue == LOW && leftIRSensorValue == LOW )
  {
    rotateMotor(MOTOR_SPEED, MOTOR_SPEED);
  }
  //If right sensor detects black line, then turn right
  else if (rightIRSensorValue == HIGH && leftIRSensorValue == LOW )
  {
    rotateMotor(-MOTOR_SPEED, MOTOR_SPEED);
  }
  //If left sensor detects black line, then turn left
  else if (rightIRSensorValue == LOW && leftIRSensorValue == HIGH )
  {
    rotateMotor(MOTOR_SPEED, -MOTOR_SPEED);
  }
  //If both the sensors detect black line, then stop
  else
  {
    rotateMotor(0, 0);
  }

  }
*/