quarta-feira, 15 de junho de 2011

Código do robô

Iremos dividir o código abaixo em funções para que fique melhor organizado. O código abaixo foi testado e funciona perfeitamente:

/*
Atenção para que o robô possa funcionar corretamente as seguintes regras devem valer:

 1. Se fótoscópio norte é o 1 (ligado na serial 0) servo do norte é o 2 (ligado na digital pwm 5);
 2. Se fótoscópio sul é o 2 (ligado na serial 1) servo do sul é o 1 (ligado na digital pwm 3);
 3. Se fótoscópio leste é o 3 (ligado na serial 3) servo do leste é o 4 (ligado na digital pwm 9);
 4. Se fótoscópio oeste é o 4 (ligado na serial 4) servo do oeste é o 3 (ligado na digital pwm 6);
*/

#include <Servo.h>                 //Esta biblioteca contem as operações para manuseio e reconhecimento dos servos
Servo servo[4];                    //Cria um vetor do tipo servo para os 4 servos (o tipo servo é tipo importado da biblioteca servo.h)

//cria constantes inteiras relativas as portas de entrada analógicas
const int pinLDRtower1 = 0;       //pino do fótoscópio 1 na porta 0, etc...
const int pinLDRtower2 = 1;
const int pinLDRtower3 = 2;
const int pinLDRtower4 = 3;

//criação das variáveis
int LDRtower[5];                   //variável do tipo número inteiro que irá receber a tensão do circuito do fotoscópio
int sensibilidade = 390;           //variável do tipo número inteiro indicando a quantidade mínima de bits recebidos pela porta analógica quando ha presença e luz
int contador;                      //variável do tipo número inteiro que servirá de contador. Será usado para comparar qual fótoscópio recebeu sinal maior
int daVez;                         //variável que irá definir qual fótoscópio o robô tem que seguir (qual é o da vez)
int fonteLuzDetectada;             //variável do tipo número inteiro que servirá de semáforo para dar inicio aos movimentos do robô caso haja uma fonte emisora de fótons

void setup(){
  //mostrar para o CI onde estão ligados os Servos
  servo[0].attach(3); //servo 0 corresponde ao primeiro servo ligado na porta 3, assim por diante...
  servo[1].attach(5);
  servo[2].attach(6);
  servo[3].attach(9);

  //inicializo cada valor do vetor que recebe os valores do fótoscópio com 0
  for(contador = 1; contador < 5; contador++){
    LDRtower[contador] = 0;
  }

  //inicializo o funcionamento da porta serial com leitura 9600bps (bps = bites por segundo)
  Serial.begin(9600);
}

void zerarVarDeControle(int daVez, int fonteLuzDetectada){
  //zerar as variáveis que iram definir qual lado o robô deve seguir
  daVez = 0;
  fonteLuzDetectada = 0;
}

void receberLeiturasAssociandoFotoscopioaPino(int LDRtower[4], int pinLDRtower1, int pinLDRtower2, int pinLDRtower3, int pinLDRtower4){
  //faz com que cada valor do vetor de fótoscopios receba as medidas de suas respectivas portas
  LDRtower[1] = analogRead(pinLDRtower1); //A torre do LDR 1 recebe os valores da porta analógica 0 (pinLDRtower1), e assim por diante...
  LDRtower[2] = analogRead(pinLDRtower2);
  LDRtower[3] = analogRead(pinLDRtower3);
  LDRtower[4] = analogRead(pinLDRtower4);
}

void funcaoDeixarRobodePe(Servo servo[4]){
  //deixa o robô em pé colocando todos os servos num angulo de 90º
  servo[0].write(90);
  servo[1].write(90);
  servo[2].write(90);
  servo[3].write(90);
}

int verificaRecepcaoLuminosa(int contador, int LDRtower[4], int sensibilidade, int fonteLuzDetectada){
  //verifica se algum fotoscopio recebeu sinal luminoso
  for (contador = 1; contador < 5; contador++){
    if (LDRtower[contador] > sensibilidade) fonteLuzDetectada = 1;
  }
  return(fonteLuzDetectada);
}

int achaMaior(int daVez, int contador, int LDRtower[4]){
  //verifica qual fotoscopio recebeu sinal maior e taxa-o como daVez
  daVez = 1;
  for (contador = 2; contador < 5; contador++) if (LDRtower[daVez] < LDRtower[contador]) daVez = contador;
  return(daVez);
}

void irEmDirecaoAoFotoscopio1(Servo servo[4]){
  //movimento de sincrono que inicia a movimentação do robô em direção ao fotoscopio 1:
 
  //deita uma perna
  servo[0].write(45);
  servo[1].write(90);
  servo[2].write(90);
  servo[3].write(90);
  delay(500);

  servo[0].write(0);
  servo[1].write(90);
  servo[2].write(90);
  servo[3].write(90);
  delay(500);

  //deita a outra
  servo[0].write(0);
  servo[1].write(135);
  servo[2].write(135);
  servo[3].write(135);
  delay(500);

  servo[0].write(0);
  servo[1].write(180);
  servo[2].write(180);
  servo[3].write(180);
  delay(100);

  //pula!
  servo[0].write(180);
  servo[1].write(0);
  servo[2].write(180);
  servo[3].write(180);
  delay(500);

  //levanta uma perna
  servo[0].write(135);
  servo[1].write(0);
  servo[2].write(135);
  servo[3].write(135);
  delay(500);

  servo[0].write(90);
  servo[1].write(0);
  servo[2].write(90);
  servo[3].write(90);
  delay(500);

  //levanta a outra
  servo[0].write(90);
  servo[1].write(45);
  servo[2].write(90);
  servo[3].write(90);
  delay(500);

  servo[0].write(90);
  servo[1].write(90);
  servo[2].write(90);
  servo[3].write(90);
  delay(500);
}

void irEmDirecaoAoFotoscopio2(Servo servo[4]){
  //movimento de sincrono que inicia a movimentação do robô em direção ao fotoscopio 2:
 
  //deita uma perna
  servo[0].write(90);
  servo[1].write(45);
  servo[2].write(90);
  servo[3].write(90);
  delay(500);

  servo[0].write(90);
  servo[1].write(0);
  servo[2].write(90);
  servo[3].write(90);
  delay(500);

  //deita a outra
  servo[0].write(135);
  servo[1].write(0);
  servo[2].write(135);
  servo[3].write(135);
  delay(500);

  servo[0].write(180);
  servo[1].write(0);
  servo[2].write(180);
  servo[3].write(180);
  delay(100);

  //pula!
  servo[0].write(0);
  servo[1].write(180);
  servo[2].write(180);
  servo[3].write(180);
  delay(500);

  //levanta uma perna
  servo[0].write(0);
  servo[1].write(135);
  servo[2].write(135);
  servo[3].write(135);
  delay(500);

  servo[0].write(0);
  servo[1].write(90);
  servo[2].write(90);
  servo[3].write(90);
  delay(500);

  //levanta a outra
  servo[0].write(45);
  servo[1].write(90);
  servo[2].write(90);
  servo[3].write(90);
  delay(500);

  servo[0].write(90);
  servo[1].write(90);
  servo[2].write(90);
  servo[3].write(90);
  delay(500);
}

void irEmDirecaoAoFotoscopio3(Servo servo[4]){
  //movimento de sincrono que inicia a movimentação do robô em direção ao fotoscopio 3:
 
  //deita uma perna
  servo[0].write(90);
  servo[1].write(90);
  servo[2].write(45);
  servo[3].write(90);
  delay(500);

  servo[0].write(90);
  servo[1].write(90);
  servo[2].write(0);
  servo[3].write(90);
  delay(500);

  //deita a outra
  servo[0].write(135);
  servo[1].write(135);
  servo[2].write(0);
  servo[3].write(135);
  delay(500);

  servo[0].write(180);
  servo[1].write(180);
  servo[2].write(0);
  servo[3].write(180);
  delay(100);

  //pula!
  servo[0].write(180);
  servo[1].write(180);
  servo[2].write(180);
  servo[3].write(0);
  delay(500);

  //levanta uma perna
  servo[0].write(135);
  servo[1].write(135);
  servo[2].write(135);
  servo[3].write(0);
  delay(500);

  servo[0].write(90);
  servo[1].write(90);
  servo[2].write(90);
  servo[3].write(0);
  delay(500);

  //levanta a outra
  servo[0].write(90);
  servo[1].write(90);
  servo[2].write(90);
  servo[3].write(45);
  delay(500);

  servo[0].write(90);
  servo[1].write(90);
  servo[2].write(90);
  servo[3].write(90);
  delay(500);
}

void irEmDirecaoAoFotoscopio4(Servo servo[4]){
  //movimento de sincrono que inicia a movimentação do robô em direção ao fotoscopio 4:
 
  //deita uma perna
  servo[0].write(90);
  servo[1].write(90);
  servo[2].write(90);
  servo[3].write(45);
  delay(500);

  servo[0].write(90);
  servo[1].write(90);
  servo[2].write(90);
  servo[3].write(0);
  delay(500);

  //deita a outra
  servo[0].write(135);
  servo[1].write(135);
  servo[2].write(135);
  servo[3].write(0);
  delay(500);

  servo[0].write(180);
  servo[1].write(180);
  servo[2].write(180);
  servo[3].write(0);
  delay(100);

  //pula!
  servo[0].write(180);
  servo[1].write(180);
  servo[2].write(0);
  servo[3].write(180);
  delay(500);

  //levanta uma perna
  servo[0].write(135);
  servo[1].write(135);
  servo[2].write(0);
  servo[3].write(135);
  delay(500);

  servo[0].write(90);
  servo[1].write(90);
  servo[2].write(0);
  servo[3].write(90);
  delay(500);

  //levanta a outra
  servo[0].write(90);
  servo[1].write(90);
  servo[2].write(45);
  servo[3].write(90);
  delay(500);

  servo[0].write(90);
  servo[1].write(90);
  servo[2].write(90);
  servo[3].write(90);
  delay(500);
}

//função padrão do Arduino, pode ser interpretada como principal, e fica em constante loop
void loop(){
  //zerar as variáveis que iram definir qual lado o robô deve seguir
  zerarVarDeControle(daVez, fonteLuzDetectada);

  //faz com que cada valor do vetor de fótoscopios receba as medidas de suas respectivas portas
  receberLeiturasAssociandoFotoscopioaPino(LDRtower, pinLDRtower1, pinLDRtower2, pinLDRtower3, pinLDRtower4);

  //deixa o robô em pé colocando todos os servos num angulo de 90º
  funcaoDeixarRobodePe(servo);

  //verifica se algum fótoscópio recebeu o sinal luminoso
  fonteLuzDetectada = verificaRecepcaoLuminosa(contador, LDRtower, sensibilidade, fonteLuzDetectada);

  //se recebeu quem recebeu o sinal maior
  if (fonteLuzDetectada == 1){
    daVez = achaMaior(daVez, contador, LDRtower);
    //uma vez que se sabe qual é o maior sinal luminoso de acordo com a variável daVez, executa-se seu respectivo movimento de acordo com o valor de daVez.
    switch (daVez) {
      case 1:    //verifica se o fotoscópio da vez é o 3
        while(daVez == 1){    //executa as funções abaixo até que o daVez não seja mais o 1
          //faz o robô andar em direção da luz recebida pelo fótoscópio 1 até que ela suma ou outro fótoscópio receba luminosidade maior
          irEmDirecaoAoFotoscopio1(servo);
 
          //zerar as variáveis que iram definir qual lado o robô deve seguir
          zerarVarDeControle(daVez, fonteLuzDetectada);

          //faz com que cada valor do vetor de fótoscopios receba as medidas de suas respectivas portas
          receberLeiturasAssociandoFotoscopioaPino(LDRtower, pinLDRtower1, pinLDRtower2, pinLDRtower3, pinLDRtower4);

          //verifica se algum fótoscópio recebeu o sinal luminoso
          fonteLuzDetectada = verificaRecepcaoLuminosa(contador, LDRtower, sensibilidade, fonteLuzDetectada);
         
          //verifica qual fotoscopio recebeu sinal maior no case de deteccao de luz
          if (fonteLuzDetectada == 1){
            daVez = achaMaior(daVez, contador, LDRtower);
          }
        }
        break;
      case 2:    //verifica se o fotoscópio da vez é o 2
        while(daVez == 2){    //executa as funções abaixo até que o daVez não seja mais o 2
          //faz o robô andar em direção da luz recebida pelo fótoscópio 2 até que ela suma ou outro fótoscópio receba luz maior
          irEmDirecaoAoFotoscopio2(servo);
 
          //zerar as variáveis que iram definir qual lado o robô deve seguir
          zerarVarDeControle(daVez, fonteLuzDetectada);
 
          //faz com que cada valor do vetor de fótoscopios receba as medidas de suas respectivas portas
          receberLeiturasAssociandoFotoscopioaPino(LDRtower, pinLDRtower1, pinLDRtower2, pinLDRtower3, pinLDRtower4);
 
          //verifica se algum fótoscópio recebeu o sinal luminoso
          fonteLuzDetectada = verificaRecepcaoLuminosa(contador, LDRtower, sensibilidade, fonteLuzDetectada);
   
          //verifica qual fotoscopio recebeu sinal maior no case de deteccao de luz
          if (fonteLuzDetectada == 1){
            daVez = achaMaior(daVez, contador, LDRtower);
          }
        }
        break;
      case 3: //verifica se o fotoscópio da vez é o 3
        while(daVez == 3){    //executa as funções abaixo até que o daVez não seja mais o 3
          //faz o robô andar em direção da luz recebida pelo fótoscópio 3 até que ela suma ou outro fótoscópio receba luminosidade maior
          irEmDirecaoAoFotoscopio3(servo);

          //zerar as variáveis que iram definir qual lado o robô deve seguir
          zerarVarDeControle(daVez, fonteLuzDetectada);
 
          //faz com que cada valor do vetor de fótoscopios receba as medidas de suas respectivas portas
          receberLeiturasAssociandoFotoscopioaPino(LDRtower, pinLDRtower1, pinLDRtower2, pinLDRtower3, pinLDRtower4);
 
          //verifica se algum fótoscópio recebeu o sinal luminoso
          fonteLuzDetectada = verificaRecepcaoLuminosa(contador, LDRtower, sensibilidade, fonteLuzDetectada);
   
          //verifica qual fotoscopio recebeu sinal maior no case de deteccao de luz
          if (fonteLuzDetectada == 1){
            daVez = achaMaior(daVez, contador, LDRtower);
          }
        }
        break;
      case 4: //verifica se o fotoscópio da vez é o 4
        while(daVez == 4){ //executa as funções abaixo até que o daVez não seja mais o 4
          //faz o robô andar em direção da luz recebida pelo fótoscópio 4 até que ela suma ou outro fótoscópio receba luz maior
          irEmDirecaoAoFotoscopio1(servo);
 
          //zerar as variáveis que iram definir qual lado o robô deve seguir
          zerarVarDeControle(daVez, fonteLuzDetectada);
   
          //faz com que cada valor do vetor de fótoscopios receba as medidas de suas respectivas portas
          receberLeiturasAssociandoFotoscopioaPino(LDRtower, pinLDRtower1, pinLDRtower2, pinLDRtower3, pinLDRtower4);
   
          //verifica se algum fótoscópio recebeu o sinal luminoso
          fonteLuzDetectada = verificaRecepcaoLuminosa(contador, LDRtower, sensibilidade, fonteLuzDetectada);
         
          //verifica qual fotoscopio recebeu sinal maior no case de deteccao de luz
          if (fonteLuzDetectada == 1){
            daVez = achaMaior(daVez, contador, LDRtower);
          }
        }
      break;
    default:
      //se nada acontecer deixa o robô de pé escrevendo um angulo de 90º em todos os servos
      funcaoDeixarRobodePe(servo);
      break;
    }
    delay(180); //tempo em microssegundos que a aplicação é repedida
  }
}

Nenhum comentário:

Postar um comentário