BANDA SELECTORA DE COLORES TCS230

International Español Hardware
5

A ver como se comporta esto

#include <Servo.h>
#define S0        2
#define S1        3
#define S2        4
#define S3        5
#define BANDA     13
#define sensorOut 6
Servo AZUL;
Servo ROJO;
int frequency   = 0;
int color       = 0;
int R, G, B;
byte estado = 0;

void setup() {
  pinMode(S0, OUTPUT);
  pinMode(S1, OUTPUT);
  pinMode(S2, OUTPUT);
  pinMode(S3, OUTPUT);
  pinMode(BANDA, OUTPUT);
  pinMode(sensorOut, INPUT);
  // Setting frequency-scaling to 20%
  digitalWrite(S0, HIGH);
  digitalWrite(S1, LOW);
  digitalWrite(BANDA, LOW);
  ROJO.attach(9);
  AZUL.attach(10);
  ROJO.write(180);
  AZUL.write(180);
  Serial.begin(9600);
}


void loop() {
  
  if (analogRead(A0) < 800) {
      switch (estado) {
          case 0: digitalWrite(BANDA, HIGH);
                  estado = 1;
                  start = millis();
                  break;
          case 1: if (millis() > start + 2000) {
                      estado = 2;
                      digitalWrite(BANDA, LOW);
                      start = millis(); 
                  }
                  break;
          case 2: if (millis() > start + 1000) {
                      estado = 0;
                  }
                  break;
      }
  }

  if (analogRead(A1) < 800)  {
      //Serial.print("SERVOS EN POSICION") ;
      AZUL.write(180);
      ROJO.write(180);
      // Serial.print("IR1");
  }
  
  sensor_color();
  
  if (R<50 & R>40 & G<120 & G>95 & B<120 & B>78) {
      ROJO.write(120);
      digitalWrite(BANDA, LOW);
      Serial.print("ROJO");
  }

  if (R<95 & R>80 & G<95 & G>85 & B<55 & B>45) {
      AZUL.write(120);
      digitalWrite(BANDA, LOW);
      Serial.print("AZUL");
  }
}

void sensor_color() {
  // Setting red filtered photodiodes to be read
  digitalWrite(S2, LOW);
  digitalWrite(S3, LOW);
  
  // Reading the output frequency
  frequency = pulseIn(sensorOut, LOW);
  R = frequency;
    delay(50);

  // Setting Green filtered photodiodes to be read
  digitalWrite(S2, HIGH);
  digitalWrite(S3, HIGH);
  
  // Reading the output frequency
  frequency = pulseIn(sensorOut, LOW);
  G = frequency;
  delay(50);
  
  // Setting Blue filtered photodiodes to be read
  digitalWrite(S2, LOW);
  digitalWrite(S3, HIGH);
  
  // Reading the output frequency
  frequency = pulseIn(sensorOut, LOW);
  B = frequency;
  
  // Printing the value on the serial monitor
  char buffer[30];
  sprintf(buffer, "R =%d G= %d B=%d", R, G, B);
  Serial.println(buffer);
  delay(50);
}