Main

const int motors[] = {};//Array de pinos PWMs || [0]->Motor esquedo  [1]->Motor direito || length = 2
const int directions[] = {};//Arras de pinos de sentidos(Motor esquerdo e direito respectivamente) || length = 4

const int trig; // Pino de trig do ultrassônico. Obs.: atribuir um valor a variável
const int echo; // Pino de echo do ultrassônico. Obs.: atribuir um valor a variável

#define OUT ..

const int s[] = { };

int red = 0;
int green = 0;
int blue = 0;


void setup() {

  for(int i = 0; i < sizeof(ft)/sizeof(int); i++)
    pinMode(ft[i], INPUT);

  for(int i = 0; i < sizeof(s)/sizeof(int); i++)
    pinMode(s[i], OUTPUT);

  pinMode(OUT, INPUT);

  digitalWrite(s[0], HIGH);
  digitalWrite(s[1], LOW);
}

void loop() {
  if(ultrassonicRead() < 5){

  }else{
    color();
    if ( ){
      Serial.println("Vermelho");
    }
    else if ( ){
      Serial.println("Azul");
    }
    else if ( ){
      Serial.println("Verde");
    }
  }

}
int ultrassonicRead(){//Retorna um valor inteiro com o valor da distância lida pelo ultrassônico
  long duracao;
  long distancia;
  digitalWrite(trig, LOW);
  delayMicroseconds(2);
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);
  duracao = pulseIn(echo, HIGH);
  distancia = duracao/58;
  delay(500);
  return distancia;
}

void motorsLeft(char sense, int pwm){//Função que define o sentido de rotação do motor esquerdo e a velocidade do giro em porcentagem
  int motor = 0;
  if(sense == 'F'){
    digitalWrite(directions[2*motor], HIGH);
    digitalWrite(directions[(2*motor)+1], LOW);
  }
  else if(sense == 'B'){
    digitalWrite(directions[2*motor], LOW);
    digitalWrite(directions[(2*motor)+1], HIGH);
  }
  analogWrite(motors[motor], 255*pwm/100);
}

void motorsRight(char sense, int pwm){//Função que define o sentido de rotação do motor direito e a velocidade do giro em porcentagem
  int motor = 1;
  if(sense == 'F'){
    digitalWrite(directions[2*motor], HIGH);
    digitalWrite(directions[(2*motor)+1], LOW);
  }
  else if(sense == 'B'){
    digitalWrite(directions[2*motor], LOW);
    digitalWrite(directions[(2*motor)+1], HIGH);
  }
  analogWrite(motors[motor], 255*pwm/100);
}

void motorsStop(int motor){//Função que desliga o motor informado
  digitalWrite(motors[motor], LOW);
  digitalWrite(directions[2*motor], LOW);
  digitalWrite(directions[(2*motor)+1], LOW);
}
void color() {
  //Rotina que le o valor das cores
  digitalWrite(s[2], LOW);
  digitalWrite(s[3], LOW);
  //count OUT, pRed, RED
  red = pulseIn(OUT, digitalRead(OUT) == HIGH ? LOW : HIGH);
  digitalWrite(s[3], HIGH);
  //count OUT, pBLUE, BLUE
  blue = pulseIn(out, digitalRead(OUT) == HIGH ? LOW : HIGH);
  digitalWrite(s[2], HIGH);
  //count OUT, pGreen, GREEN
  green = pulseIn(OUT, digitalRead(OUT) == HIGH ? LOW : HIGH);
}
int readFt(int s){
  return digitalRead(s);
}