Lógica para o hardware de comparação de cores

bracomecanico.ino

// bracomecanico-0.0.0.ino
// 
// Projeto Braço Mecânico
//
// Revisão 0.0.0
// Data: Abril de 2017
// Universidade: Doctum
// Disciplina: Integradora
// Professor(a): Danielle Alcantara
// Autores:
//   * Vinicius Dominato
//   *
//   *
//   *
// 

#include <Servo.h>
#include <math.h>

/**
 * Pino do sensor LDR.
 */
#define LDR A0

/**
 * Pino do LED vermelho.
 */
#define LED_VERMELHO 9

/**
 * Pino do LED verde.
 */
#define LED_VERDE 10

/**
 * Pino do LED azul.
 */
#define LED_AZUL 11 

/**
 * Tempo de ajuste do LDR.
 */
#define TEMPO_LDR_MS 420

/**
 * Quantidade de leituras (amostras).
 */
#define Q_LEITURAS 20

/**
 * Quantidade de referências (cores).
 */
#define Q_CORES 10

/**
 * Valores para do CodigoCor
 */
#define COR_DESCONHECIDA -1
#define COR0 0
#define COR1 1
#define COR2 2
#define COR3 3
#define COR4 4
#define COR5 5
#define COR6 6
#define COR7 7
#define COR8 8
#define COR9 9

/**
 * Indicadores do status da calibração.
 */
#define CALIBRADA true
#define NAO_CALIBRADA false

/**
 * Caractere de escape.
 * 
 * É enviado para parar a execucao em loop.
 */
#define ESC 'q'

/**
 * Espectro da cor (ou frequência). È um número real que representa 
 * o nível elétrico transduzido pelo sensor como a frêquencia luminosa
 * incidente sobre o mesmo.
 * 
 * Campos:
 *   * média - Média das amostras (valores analógicos) do espectro.
 *   * desvio - Desvio padrão das amostras do espectro.
 */
struct Espectro
{
  double media;
  double desvio;
};

/**
 * Três "espectros" (vermelho, verde e azul) que compõem uma "cor".
 * 
 * Campos:
 *   * vermelho - Espectro vermelho.
 *   * verde    - Espectro verde.
 *   * azul     - Espectro azul.
 */
struct Cor
{
  Espectro vermelho;
  Espectro verde;
  Espectro azul;
};

/**
 * Uma referência é um valor de cor que ficará armazenado na
 * memória para comparação e identificação de outras cores.
 * 
 * Campos:
 *   * cor - a cor que será comparada
 *   * status - se esta referência já foi calibrada. Ou seja,
 *     se já possui seus valores lidos pelo sensor.
 */
struct Referencia
{
  Cor cor;
  bool status;
};

/**
 * Código da cor. Pode assumir os valores:
 *   COR0, COR1, COR2, COR3, COR4, 
 *   COR5, COR6, COR7, COR8, COR9 e
 *   COR_DESCONHECIDA
 */
typedef int CodigoCor;

/**
 * Conjunto de referências (cores).
 */
Referencia referencias[Q_CORES];

/**
 * Armazena o caractere de comando enviado pelo terminal.
 */
char ch;

/**
 * Número de vezes em que a ação principal foi executada.
 */
unsigned short execucoes = 0;

/**
 * Janela do espectro.
 * 
 * Se este número for baixo (< 10), permitirá a diferenciação mais precisa
 * de duas cores próximas. Se este número for mais alto (> 20), identificará 
 * tonalidades próximas como a mesma cor. A janela do espectro pode ser 
 * incrementada ou decrementada no programa conforme o comportamento desejado.
 */
unsigned short janela = 20;

/**
 * Garra do braço mecânico.
 */
Servo garra;

/**
 * Pulso do braço mecânico.
 */
Servo pulso;

/**
 * Giro do pulso do braço mecânico.
 */
Servo pulsogiro;

/**
 * Cotovelo do braço mecânico.
 */
Servo cotovelo;

/**
 * Ombro do braço mecânico.
 */
Servo ombro1;

/**
 * Ombro do braço mecânico.
 */
Servo ombro2;

/**
 * Base do braço mecânico.
 */
Servo base;

/**
 * Conrigurações iniciais mecânico.
 *
 * Esta função é chamada pelo Arduino e sua definição é obrigatória.
 */
void setup()
{
  // Define os pinos do LED RGB como saída.
  pinMode(LED_VERMELHO, OUTPUT);
  pinMode(LED_VERDE, OUTPUT);
  pinMode(LED_AZUL, OUTPUT);
  
  // Certifica-se que os LED se iniciem apagados
  apagaLedVermelho();
  apagaLedVerde();
  apagaLedAzul();

  bracoRetornaPosicaoInicial();
  
  // Inicializa a serial
  Serial.setTimeout(0);
  Serial.begin(9600);

  exibeInfoProjeto();
  exibeMenuAjuda();
}

/**
 * Retorna o braço à posição inicial.
 */
void bracoRetornaPosicaoInicial()
{
  garra.write(90);
  pulso.write(0);
  pulsogiro.write(90);
  cotovelo.write(0);
  ombro1.write(120);
  ombro2.write(120);
  base.write(30);  
}

/**
 * Calcula a média das amostras.
 * 
 * Parâmetros:
 *   * leituras: conjunto de amostras lidas pelo sensor.
 *   
 * Retorna:
 *   A média das amostras.
 */
double media(int leituras[])
{
    double soma = .0;
    
    for (byte i = 0; i < Q_LEITURAS; i++)
    {
      soma = soma + (double) leituras[i];
    }

    return (soma / (double) Q_LEITURAS);
}

/**
 * Calcula o desvio padrão das amostras.
 * 
 * Parâmetros:
 *   * leituras: conjunto de amostras lidas pelo sensor.
 *   * media: media já cauculada das amostras
 *   
 * Retorna:
 *   O desvio padrão das amostras.
 */
double desvioPadrao(int leituras[], double media)
{
    double soma = .0;
 
    for (byte i = 0; i < Q_LEITURAS; i++)
    {
      soma += square(((double) leituras[i]) - media);
    }

    return sqrt(soma / Q_LEITURAS);
}

/**
 * Faz uma leitura do sensor LDR.
 * 
 * Retorna:
 *   Um inteiro como valor da leitura (sinal analógico) do sensor.
 *   Pode variar de 0 a 1023.
 */
int leSensor()
{
  return analogRead(LDR);
}

/**
 * Acende o LED vermelho.
 */
void acendeLedVermelho()
{
  digitalWrite(LED_VERMELHO, LOW);
}

/**
 * Acende o LED verde.
 */
void acendeLedVerde()
{
  digitalWrite(LED_VERDE, LOW);
}

/**
 * Acende o LED azul.
 */
void acendeLedAzul()
{
  digitalWrite(LED_AZUL, LOW);
}

/**
 * Apaga o LED vermelho.
 */
void apagaLedVermelho()
{
  digitalWrite(LED_VERMELHO, HIGH); 
}

/**
 * Apaga o LED azul.
 */
void apagaLedAzul()
{
  digitalWrite(LED_AZUL, HIGH); 
}

/**
 * Apaga o LED verde.
 */
void apagaLedVerde()
{
  digitalWrite(LED_VERDE, HIGH);
}

/**
 * Bloqueia o programa até ler um caractere da serial.
 */
char leCaractere()
{
  // Dispensa bytes do buffer de leitura
  while(Serial.read() != -1);

  // Espera até que existam bytes no buffer
  while(!Serial.available());

  // retorna o caractere lido
  return Serial.read();
}

/**
 * Lê os valores RGB que compõem uma cor
 */
void leCor(struct Cor *cor)
{
  acendeLedVermelho();
  delay(TEMPO_LDR_MS);
  cor->vermelho = leEspectro();
  apagaLedVermelho();

  acendeLedVerde();
  delay(TEMPO_LDR_MS);
  cor->verde = leEspectro();
  apagaLedVerde();

  acendeLedAzul();
  delay(TEMPO_LDR_MS);
  cor->azul = leEspectro();
  apagaLedAzul();
}

/**
 * Lê um espectro de luz.
 * 
 * @return espectro lido.
 */
struct Espectro leEspectro()
{
  Espectro e;
  double _media, desvio;
  int leituras[Q_LEITURAS];

  for (byte i = 0; i < Q_LEITURAS; i++)
  {
      leituras[i] = leSensor();
  }
  
  _media = media(leituras);
  desvio = desvioPadrao(leituras, _media);

  e.media = _media;
  e.desvio = desvio;
  
  return e;
}

/**
 * Calibra uma referência de cor
 * 
 * @param r indicd da referência
 */
void calibraReferencia(byte r)
{
  Cor *cor = &referencias[r].cor;
  
  Serial.print(F("  | Calibrando referencia "));
  Serial.print(r);
  Serial.println(F("..."));
  Serial.println(F("  |"));
  
  leCor(cor);
  exibeInfoCor(cor);

  referencias[r].status = CALIBRADA;
  
  Serial.println(F("  |"));
  Serial.println(F("  | Referencia calibrada com sucesso!"));
}

/**
 * Exibe informações sobre o projeto.
 */
void exibeInfoProjeto()
{
  Serial.println(F("  | Projeto Braco Mecanico"));
  Serial.println(F("  | Revisao: 0.0.0"));
  Serial.println(F("  | Data: Abril de 2017"));
  Serial.println(F("  | Universidade: Doctum"));
  Serial.println(F("  | Disciplina: Integradora"));
  Serial.println(F("  | Professor(a): Danielle Alcantara")); 
  Serial.println(F("  | Autores:"));  
  Serial.println(F("  |    * Vinicius Dominato"));
  Serial.println(F("  |    * Eric Cartman"));
  Serial.println(F("  |    * Kenny McCormick"));
  Serial.println(F("  |    * Kyle Broflovsky"));
  Serial.println(F("  |    * Stan Marsh"));
  Serial.println(F("  |"));
}

/**
 * Exibe o menu de ajuda.
 */
void exibeMenuAjuda()
{
  Serial.println(F("  | Ajuda:"));
  Serial.println(F("  |"));
  Serial.println(F("  | Digite um dos caracteres abaixo para"));
  Serial.println(F("  | executar a acao correspondente:"));
  Serial.println(F("  |"));
  Serial.println(F("  |   0 -> Calibra a referencia (cor) 0."));
  Serial.println(F("  |   1 -> Calibra a referencia (cor) 1."));
  Serial.println(F("  |   2 -> Calibra a referencia (cor) 2."));
  Serial.println(F("  |   3 -> Calibra a referencia (cor) 3."));
  Serial.println(F("  |   4 -> Calibra a referencia (cor) 4."));
  Serial.println(F("  |   5 -> Calibra a referencia (cor) 5."));
  Serial.println(F("  |   6 -> Calibra a referencia (cor) 6."));
  Serial.println(F("  |   7 -> Calibra a referencia (cor) 7."));
  Serial.println(F("  |   8 -> Calibra a referencia (cor) 8."));
  Serial.println(F("  |   9 -> Calibra a referencia (cor) 9."));
  Serial.println(F("  |   h -> Exibe o menu de ajuda (este menu)."));
  Serial.println(F("  |   n -> Executa acao do braco 1 vez."));
  Serial.println(F("  |   m -> Executa acao do braco em loop."));
  Serial.println(F("  |   q -> Sai da execucao em loop."));
  Serial.println(F("  |   + -> Incrementa janela do espectro."));
  Serial.println(F("  |   - -> Decrementa janela do espectro."));
  Serial.println(F("  |   s -> Exibe informacoes sobre o status do programa."));
  Serial.println(F("  |   p -> Exibe informacoes sobre o projeto."));
}

/**
 * Exibe informações sobre um espectro.
 * 
 * Parâmetros:
 *    * e - Espectro a exibir informações.
 */
void exibeInfoEspectro(struct Espectro e)
{
  Serial.print(F("media: "));
  Serial.print(e.media);
  Serial.print(F(" / desvio padrao: "));
  Serial.print(e.desvio);
}

/**
 * Exibe informações sobre uma cor.
 * 
 * Parâmetros:
 *   * c - Cor a exibir as informações.
 */
void exibeInfoCor(struct Cor *c)
{
  Serial.print(F("  | Espectro VERMELHO: "));
  exibeInfoEspectro(c->vermelho);
  Serial.println();
  
  Serial.print(F("  | Espectro VERDE   : "));
  exibeInfoEspectro(c->verde);
  Serial.println();

  Serial.print(F("  | Espectro AZUL    : "));
  exibeInfoEspectro(c->azul);
  Serial.println();
}

/**
 * Exibe informações das calibrações feitas.
 */
void exibeInfoCalibracao()
{
  for (byte c; c < Q_CORES; c++)
  { 
    if (referencias[c].status == NAO_CALIBRADA) continue; // Pula as referências não calibradas.
    
    Serial.print(F("  | Referencia (cor) "));
    Serial.print(c);
    Serial.println(F(":"));
    Serial.println(F("  |"));
     
    exibeInfoCor(&referencias[c].cor);
    
    Serial.println(F("  |"));
  }
}

/**
 * Exibe o valor da janela do espectro.
 */
void exibeInfoJanela()
{
  Serial.print(F("  | Janela do espectro: "));
  Serial.println(janela);
}

/**
 * Exibe informação sobre o número de vezes em que a ação
 * foi executada
 */
void exibeInfoExecucoes()
{
  Serial.print(F("  | Numero de execucoes: "));
  Serial.println(execucoes);
}

/**
 * Exibe informação sobre o número de amostras por espectro.
 */
void exibeInfoLeituras()
{
  Serial.print(F("  | Amostras por espectro: "));
  Serial.println(Q_LEITURAS);   
}

/**
 * Exibe informações e parâmetros gerais do programa.
 */
void exibeInfoStatus()
{
  Serial.println(F("  | Status:"));
  Serial.println(F("  |"));
  
  exibeInfoJanela();
  exibeInfoLeituras();
  exibeInfoExecucoes();
   
  Serial.println(F("  |"));
  
  exibeInfoCalibracao();
}

/**
 * Detecta uma cor.
 * 
 * Retorna:
 *   Retorna um inteiro de 0 a 9 indicando a cor detectada, 
 *   ou -1 se a cor for desconhecida.
 */
CodigoCor detectaCor()
{
  Cor cor;

  //Serial.println(F("  | Lendo cor..."));
  leCor(&cor);

  //Serial.println(F("  | Comparando cores..."));
  return comparaCores(&cor);
}

/**
 * Ação principal do programa.
 */
void executa()
{
  braco();
  execucoes++;
}

/**
 * A lógica do braço.
 */
void braco()
{
  // 
  // APAGUE O CÒDIGO DESTA FUNÇÂO E ESCREVA A LÓGICA DO BRACO AQUI.
  //
  // EXEMPLO:
  // 
  // CodigoCor cor = detectaCor();
  // 
  // switch(cor)
  // {
  //    case COR1:
  //       mexeOBracoParaFunil1();
  //       break;
  //    case COR2:
  //       mexeOBracoParaFunil2();
  //       break;
  //    case COR3:
  //       mexeOBracoParaFunil3();
  //       break;
  //    case COR_DESCONHECIDA:
  //       executaAcaoParaCorDesconhecida();
  //       break;
  //    default:
  //       break;  
  //  }
  //
  //

  bracoRetornaPosicaoInicial();

  CodigoCor cor  = detectaCor();
    
  Serial.println(F("  | Detectando cor..."));

  if (cor == COR_DESCONHECIDA)
  {
    Serial.println(F("  | Cor desconhecida!"));
  }
  else
  {
    Serial.print(F("  | Cor detectada: "));
    Serial.println(cor);  
  }
}

/**
 * Verifica se o sinal de escape foi enviado.
 * 
 * Retorna:
 *   true se o sinal foi enviado;
 *   false se o sinal não foi enviado.
 */
bool sinalDeEscapeEnviado()
{
  while ((ch = Serial.read()) != -1)
  {
      if (ch == ESC) return true;
  }

  return false;
}

/**
 * Executa ação principal em loop.
 * 
 * Para ao final da última uma execução se o sinal de escape
 * for enviado.
 */
void executaEmLoop()
{
  Serial.println(F("  | Executando acao em loop."));

  while (1)
  {
    executa();
    exibeInfoExecucoes();

    if (sinalDeEscapeEnviado())
    {
      Serial.println(F("  | Sinal de escape enviado. Execucao finalizada."));
      break;
    }
  }
}

/**
 * Compara uma cor contra uma referência de cor.
 * 
 * Parâmetros
 *   * lida - A cor lida a ser comparada.
 *   * ref -  Referência para a comparação.
 *   
 * Retorna:
 *   true se as cores são as mesmas ou false se as cores forem diferentes
 */
bool comparaCor(struct Cor *lida, struct Cor *ref)
{
  return
    lida->vermelho.media < (ref->vermelho.media + janela) && (lida->vermelho.media > ref->vermelho.media - janela) &&
    lida->verde.media    < (ref->verde.media + janela)    && (lida->verde.media    > ref->verde.media - janela)    &&
    lida->azul.media     < (ref->azul.media + janela)     && (lida->azul.media     > ref->azul.media - janela);
}

/**
 * Compara uma cor contra todas as cores (referências) calibradas
 * 
 * Parâmetros:
 *   * cor - Cor a ser comparada.
 *
 * Retorna:
 *   Retorna um inteiro de 0 a 9 indicando a cor detectada, 
 *   ou -1 se a cor for desconhecida.
 */
CodigoCor comparaCores(struct Cor *cor)
{
  for (byte i = 0; i < Q_CORES; i++)
  {
    if (referencias[i].status == NAO_CALIBRADA) continue; // Pula as referências não calibradas.
    if (comparaCor(cor, &referencias[i].cor)) return i; 
  }
  return -1;
}

/**
 * Decrementa o número da janela do espectro.
 */
void decrementaJanela()
{
  janela = 0 ? janela : janela - 1;
  exibeInfoJanela();
}

/**
 * Incrementa o número da janela do espectro
 */
void incrementaJanela()
{
  janela++;
  exibeInfoJanela();
}

/**
 * Converte um digito (caracteres de '0' a '9'
 * para um número inteiro (de 0 a 9)
 * 
 * Parâmetros:
 *   * ch - Caractere do dígito.
 *   
 * Retorna:
 *   O inteiro correspondente ao dígito.
 */
byte converteDigitoParaInt(char ch)
{
  return ch - '0';
}

/**
 * Executa em loop.
 * 
 * Esta função é chamada pelo Arduino e sua definição é obrigatória.
 */
void loop()
{
  Serial.print("> ");
  ch = leCaractere();
  Serial.println();
  
  switch(ch)
  {
      case 'h':
        exibeMenuAjuda();
        break;
      case '0':
      case '1':
      case '2':
      case '3':
      case '4':
      case '5':
      case '6':
      case '7':
      case '8':
      case '9':
        calibraReferencia(converteDigitoParaInt(ch));
        break;
      case 's':
        exibeInfoStatus();
        break;
      case 'p':
        exibeInfoProjeto();
        break;
      case 'n':
        executa();
        break;
      case 'm':
        executaEmLoop();
        break;
      case '+':
        incrementaJanela();
        break;
      case '-':
        decrementaJanela();
        break;
      default:
        Serial.println(F("  | Comando desconhecido!"));
        exibeMenuAjuda();
        break;
  }
}