estou montando um robô autônomo para labirinto , a parte de deslocamento no labirinto esta ok, mas não estou conseguindo fazer o sensor de cor (tcs3200) reconhecer uma cor e acionar  o servo>

Alguem poderia me ajudar!!

    //#include <LiquidCrystal.h> // declara a utilização da biblioteca LiquidCrystal
//#include <Servo.h>
 
#define trigfrente 12
#define echofrente 13
#define trigdireita 11
#define echodireita 10
#define trigesquerda 9
#define echoesquerda 8
#define mot1atras 6
#define mot1frente 7
#define mot2atras 4
#define mot2frente 5
//#define Luz_Fundo  28
//Servo motor;
//Pinos de conexao do modulo TCS230 
const int s0 = 34; 
const int s1 = 35; 
const int s2 = 36; 
const int s3 = 37; 
const int out = 38;
int red = 0; 
int green = 0; 
int blue = 0; 

int duracao, distancia,duracao2, distancia2,duracao3, distancia3;
int di;
//LiquidCrystal lcd(27, 26, 25, 24, 23, 22);

void setup ( ) 
{
  Serial.begin (9600);
  pinMode(trigfrente, OUTPUT);
  pinMode(echofrente, INPUT);
  pinMode(trigdireita, OUTPUT);
  pinMode(echodireita, INPUT);
  pinMode(trigesquerda, OUTPUT);
  pinMode(echoesquerda, INPUT);
  pinMode(mot1frente,OUTPUT);
  pinMode(mot1atras,OUTPUT);
  pinMode(mot2frente,OUTPUT);
  pinMode(mot2atras,OUTPUT);
  di=0;
  pinMode(s0, OUTPUT); 
  pinMode(s1, OUTPUT); 
  pinMode(s2, OUTPUT); 
  pinMode(s3, OUTPUT); 
  pinMode(out, INPUT);
  digitalWrite(s0, HIGH); 
  digitalWrite(s1, HIGH); 

   //lcd.begin(16, 2); // Iniciando o objeto "lcd" de 2 linhas e 16 colunas
//pinMode(Luz_Fundo,OUTPUT); //define o pino como saída
//digitalWrite(Luz_Fundo,HIGH); // Liga a luz do display.
//motor.attach(3);
}
void disfrente()
{
  digitalWrite(trigfrente, HIGH);
  delayMicroseconds(1000);
  digitalWrite(trigfrente, LOW);
  duracao = pulseIn(echofrente, HIGH);
  distancia = ((duracao/2) / 29.1)+1;
  Serial.print(distancia);
  Serial.println("cm");
  delay(10);
}
void direita()
{
  digitalWrite(trigdireita, HIGH);
  delayMicroseconds(1000);
  digitalWrite(trigdireita, LOW);
  duracao2 = pulseIn(echodireita, HIGH);
  distancia2 = ((duracao2/2) / 29.1)+1;
  Serial.print(distancia2);
  Serial.println("cm");
  delay(10);
}
void esquerda()
{
  digitalWrite(trigesquerda, HIGH);
  delayMicroseconds(1000);
  digitalWrite(trigesquerda, LOW);
  duracao3 = pulseIn(echoesquerda, HIGH);
  distancia3 = ((duracao3/2) / 29.1)+1;
  Serial.print(distancia3);
  Serial.println("cm");
  delay(10);
}
 void motesquerda()
{
 analogWrite(mot1atras,80);
   analogWrite(mot1frente,0);
        analogWrite(mot2atras,0);
 analogWrite(mot2frente,100);
}
void motdireita()
{
 analogWrite(mot2atras,80);
 analogWrite(mot2frente,0);
        analogWrite(mot1atras,0);
 analogWrite(mot1frente,100);
}
void motfrente()
{
 analogWrite(mot1frente,80);
 analogWrite(mot1atras,0);
 analogWrite(mot2frente,80);
 analogWrite(mot2atras,0);
}
void motatras()
{
 analogWrite(mot1frente,0);
 analogWrite(mot1atras,120);
 analogWrite(mot2frente,0);
 analogWrite(mot2atras,120);
}
void motstop()
{
 analogWrite(mot1frente,0);
 analogWrite(mot1atras,0);
 analogWrite(mot2frente,0);
 analogWrite(mot2atras,0);
}
void loop()
{
    color(); //Chama a rotina que le as cores 
  //Mostra no serial monitor os valores detectados 
  Serial.print("Vermelho :"); 
  Serial.print(red, DEC); 
  Serial.print(" Verde : "); 
  Serial.print(green, DEC); 
  Serial.print(" Azul : "); 
  Serial.print(blue, DEC); 
  Serial.println();
 //Verifica se a cor vermelha foi detectada 
  if (red < blue && red < green && red > 50) 
  { 
   Serial.println("Vermelho");  
  } 

  //Verifica se a cor azul foi detectada 
  else if (blue < red && blue < green)  
  { 
   Serial.println("Azul");
  }
   //Verifica se a cor verde foi detectada 
  else if (green < red && green < blue) 
  { 
     Serial.println("Verde");
  }
  Serial.println(); 
 
 
 
 

  //readSensor();
  //motor.write(160);
 //delay(1000);
 // motor.write(16);
  //delay(1000);
  //lcd.setCursor(0,0); // seta o cursor para: (coluna = 0, linha = 0)
//lcd.print("GEISON");
//lcd.setCursor(0,1); // seta para linha 1, ou seja, a linha de baixo
//lcd.print("SARVALAIO");
//lcd.print(millis()/1000); // escreve os segundos passados.
 // delay(3000);
  //while(1)
  {
disfrente();
  direita();
  esquerda();
  if(distancia >=30)
  {
  motfrente();
 }
 else if(distancia<=8)
 {
   motatras();
 }
 else if((distancia<=23)&&(distancia>9))
  {
    motstop();
    if(distancia2 >= distancia3)
    {
      motdireita();
    }
    else
    motesquerda();
  }
 
  if(distancia3 <=8 && distancia3 >0 )
    {
      motstop();
      motdireita();
    }
    if(distancia2 <=8 && distancia2 >0 )
    {
      motstop();
   motesquerda();
    }
 
  }
 }

     
void color() 
{ 
  //Rotina que le o valor das cores 
  digitalWrite(s2, LOW); 
  digitalWrite(s3, LOW); 
  //count OUT, pRed, RED 
  red = pulseIn(out, digitalRead(out) == HIGH ? LOW : HIGH); 
  digitalWrite(s3, HIGH); 
  //count OUT, pBLUE, BLUE 
  blue = pulseIn(out, digitalRead(out) == HIGH ? LOW : HIGH); 
  digitalWrite(s2, HIGH); 
  //count OUT, pGreen, GREEN 
  green = pulseIn(out, digitalRead(out) == HIGH ? LOW : HIGH); 
}