Cursos

Ferramentas do usuário

Ferramentas do site

cursos:introrobotica:2019-1:grupo04:codigo_tp2
#include <math.h>
#include <LiquidCrystal.h>
#include <Adafruit_MotorShield.h>
 
 
Adafruit_MotorShield AFMS = Adafruit_MotorShield(); 
 
Adafruit_DCMotor *motor1 = AFMS.getMotor(1);
Adafruit_DCMotor *motor2 = AFMS.getMotor(2);
 
LiquidCrystal lcd(8, 9, 4, 5, 6, 7);
// ##################################################################
 
// constantes para identificar as portas digitais de cada LED
int RED = 23;
int GREEN = 25;
int BLUE = 27;
 
// porta analogica do LDR
int LDR = 8;
 
// Matriz com os pinos onde o LED RGB vai ser conectado 
int matrizPinodosLEDS[] = {RED,GREEN,BLUE}; 
 
// Uma variável booleana para indicar se o sensor foi calibrado 
boolean balanco = false; 
 
//Matriz de floats para armazenar o valor das cores 
float matrizCores[] = {   0,0,0}; 
float matrizBranco[] = {   0,0,0}; 
float matrizPreto[] = {   0,0,0}; 
 
//Variável para armazenar a média das leitura 
int mediaLeituras; 
 
// ###################################################################
 
int v1 = 0; // usado para aumentar a velocidade do motor 1
int v2 = 0; // usado para aumentar a velocidade do motor 2
float t_gir180 = 2.1;
float t_dir90 = 1.025;
float t_esq90 = 0.8;
float t_dir45 = 1.5;
float t_frente = 2.3;
float t_re = 2;
 
int vel1 = 97; // velocidade inicial do motor 1 - 140
int vel2 = 130;// velocidade inicial do motor 2 - 160
int vel1_frente = 0;
int vel2_re = 0;
 
 
 
void setup()   
{  
  lcd.begin(16, 2);  
  lcd.setCursor(0,0);  
  lcd.print("Bora Time, Foco!");  
  lcd.setCursor(0,1);  
  lcd.print("            ");    
  Serial.begin(9600);
  AFMS.begin();
  motor1->setSpeed(vel1);
  motor2->setSpeed(vel2);
  motor1->run(RELEASE);
  motor2->run(RELEASE);
 
  pinMode(RED,OUTPUT);//LED VERMELHO   
  pinMode(GREEN,OUTPUT);//LED VERDE   
  pinMode(BLUE,OUTPUT);//LED AZUL   //Inicia a comunicação serial
 
  // Verifica se o Balanco de branco foi calibrado   
  checaBalanco(); 
}
 
void dir90 (float t){
  t = t*1000;
  motor1->run(BACKWARD);
  motor2->run(FORWARD);
  delay(t);
}
 
void esq90 (float t){
  t = t*1000;
  motor1->run(FORWARD);
  motor2->run(BACKWARD);
  delay(t);
}
 
void dir45 (float t){
  t = t*1000;
  motor1->run(BACKWARD);
  motor2->run(FORWARD);
  delay(t);
}
 
void para(){
  motor1->run(RELEASE);
  motor2->run(RELEASE);
  delay(500);
}
 
void frente(float t){
  t = t*1000;
  motor1->run(BACKWARD);
  motor2->run(BACKWARD);
  delay(t);
}
 
void re(float t){
  t = t*1000;
  motor1->run(FORWARD);
  motor2->run(FORWARD);
  delay(t);
}
 
void direita(float t){
  t = t*1000;
  motor1->run(BACKWARD);
  motor2->run(RELEASE);
  delay(t);
}
 
void esquerda(float t){
  t = t*1000;
  motor1->run(RELEASE);
  motor2->run(BACKWARD);
  delay(t);
}
 
// Aumenta velocidade motor 1
void AumentaVel1() {
  if (vel1 < 210){
    vel1 = vel1 + 1;
  }
  else{
    vel1 = 100;
 }
} 
 
// diminui velocidade motor 1
void AumentaVel2() {
  if (vel1 < 210){
    vel1 = vel1 - 1;
  }
  else{
    vel1 = 100;
 }
} 
 
 
void reta(){
  frente(t_frente);
  para();
  gira(t_gir180);
  para();
  frente(t_frente);
  para();
}
 
void triangulo(){
  frente(t_frente);
  para();
  gira(t_dir45);
  para();
  float t = 2.8;
  frente(t);
  para();
  gira(t_dir45);
  para();
  frente(t_frente);
  para();
  gira(t_dir90);
  para();
}
 
void quadrado(){
  frente(t_frente);
  para();
  gira(t_dir90);
  para();
  frente(t_frente);
  para();
  gira(t_dir90);
  para();
  frente(t_frente);
  para();
  gira(t_dir90);
  para();
  frente(t_frente);
  para();
  gira(t_dir90);
  para();
}
 
// gira horario
void gira(float t){
  t = t*1000;
  motor1->run(BACKWARD);
  motor2->run(FORWARD);
  delay(t);
}
 
void giraesquerda(float t){
  t = t*1000;
  motor1->run(FORWARD);
  motor2->run(BACKWARD);
  delay(t);
}
 
//gira antihorario
void giraanti(float t){
  t = t*1000;
  motor1->run(FORWARD);
  motor2->run(BACKWARD);
  delay(t);
}
 
 
 
void esquerda(){
  int tempo=0;
 
  para();
  esquerda(20);
  para();
 
}
 
void loop(){  
  int botao;  
  botao = analogRead (0);  //Leitura do valor da porta analógica A0
  lcd.setCursor(0,1);  
  lcd.print(vel1+v1);
  lcd.setCursor(12,1);
  lcd.print (vel2+v2);
 
 
 
  if (botao < 100) {  
    // ---------------------- BOTAO RIGHT -----------------
    lcd.setCursor(0,0);  
    lcd.print("Quadrado  ");  
    lcd.setCursor(0,1);  
    lcd.print("                "); 
    quadrado();
    motor1->run(RELEASE);
    motor2->run(RELEASE);
    delay(500);
  }  
  else if (botao < 200) {
    // ---------------------- BOTAO UP -----------------
    delay(300);
 
    lcd.setCursor(0,0);  
    lcd.print("Triangulo  ");  
    lcd.setCursor(0,1);  
    lcd.print("                "); 
    triangulo();
    motor1->run(RELEASE);
    motor2->run(RELEASE);
 
  }  
  else if (botao < 400){ 
    // -------------------- BOTAO DOWN --------------   
    //Checa qual é a cor   
    checaCores();   
 
    delay (300);
  }  
  else if (botao < 600){ 
    // -------------------- BOTAO LEFT -------------- 
    lcd.setCursor(0,0);  
    lcd.print("Reta  ");  
    lcd.setCursor(0,1);  
    lcd.print("                "); 
    reta(); // -> chama a função pra fazer o robo andar reto, girar e voltar reto
    motor1->run(RELEASE);
    motor2->run(RELEASE);
      delay(500);
  }
  else if (botao < 800){  
    // BOTAO SELECT
  while(checaCoresParaAndar() == 0 && checaCoresParaAndar() == 3 
  && checaCoresParaAndar() == 4 && checaCoresParaAndar() == 5){
    motor1->run(FORWARD);
    motor2->run(FORWARD);
    if(checaCoresParaAndar() == 3){
      gira(t_gir180);
      motor1->run(FORWARD);
      motor2->run(FORWARD);
    }
    if(checaCoresParaAndar() == 4){
      giraesquerda(t_esq90);
      motor1->run(FORWARD);
      motor2->run(FORWARD);
    }
    if(checaCoresParaAndar() == 5){
      gira(t_dir90);
      motor1->run(FORWARD);
      motor2->run(FORWARD);
    }
  }
  if(checaCoresParaAndar() == 1){
    motor1->run(RELEASE);
    motor2->run(RELEASE);
  }
  if(checaCoresParaAndar() == 2){
    gira(t_gir180*2);
  }
  }  
}
 
//##############################################################################
 
// Verifica se o Balanco de branco foi calibrado
void checaBalanco() {   
  //checa se o balanço de branco foi feito   
  if(balanco == false){     
  confBalanco();   
  } 
} 
 
//Configura o balanco de Branco 
void confBalanco(){   
 
  //Calibrando o branco!     
  lcd.setCursor(0,0);  
  lcd.print("Calibra Branco  ");  
  lcd.setCursor(0,1);  
  lcd.print("                ");     
  delay(5000);   
  for(int i = 0;i<=2;i++){     
    digitalWrite(matrizPinodosLEDS[i],HIGH);     
    delay(100);     
    mediaSensor(5);               
    matrizBranco[i] = mediaLeituras;     
    digitalWrite(matrizPinodosLEDS[i],LOW);     
    delay(100);   
  }   
  //Calibrando o preto!   
  lcd.setCursor(0,0);  
  lcd.print("Calibra Preto  ");  
  lcd.setCursor(0,1);  
  lcd.print("                ");   
  delay(5000);   
  for(int i = 0;i<=2;i++){     
    digitalWrite(matrizPinodosLEDS[i],HIGH);     
    delay(100);     
    mediaSensor(5);     
    matrizPreto[i] = mediaLeituras;     
    digitalWrite(matrizPinodosLEDS[i],LOW);     
    delay(100);   
  }   
 
  //Avisa que calibrou o sensor   
  lcd.setCursor(0,0);  
  lcd.print("Calibrado       ");  
  lcd.setCursor(0,1);  
  lcd.print("               ");    
  balanco = true;   
  delay(3000); 
} 
 
 
//Checa a cor 
void checaCores(){ 
 
    //testa com o led vermelho: 
    digitalWrite(RED, HIGH);
    digitalWrite(matrizPinodosLEDS[0],HIGH);     
    delay(200);     
    mediaSensor(5);                       
    matrizCores[0] = mediaLeituras;   
    Serial.print("Vermelho: "); 
    Serial.print(matrizCores[0]);
    Serial.print("\n");          
    float cinzaDif = matrizBranco[0] - matrizPreto[0];  
    /*Serial.print("CinzaDif[0]: "); 
    Serial.print(cinzaDif);
    Serial.print("\n");    */
    matrizCores[0] = (matrizCores[0] - matrizPreto[0])/(cinzaDif)*255;      
    digitalWrite(matrizPinodosLEDS[0],LOW);   
    digitalWrite(RED, LOW);  
 
    //testa com o led verde:  
    digitalWrite(GREEN, HIGH);
    digitalWrite(matrizPinodosLEDS[1],HIGH);     
    delay(200);     
    mediaSensor(5);                       
    matrizCores[1] = mediaLeituras; 
    Serial.print("Verde: "); 
    Serial.print(matrizCores[1]);
    Serial.print("\n");             
    cinzaDif = matrizBranco[1] - matrizPreto[1];   
    Serial.print("CinzaDif[1]: "); 
    Serial.print(cinzaDif);
    Serial.print("\n");   
    matrizCores[1] = (matrizCores[1] - matrizPreto[1])/(cinzaDif)*255;      
    digitalWrite(matrizPinodosLEDS[1],LOW);   
    digitalWrite(GREEN, LOW);   
 
    //testa com o led azul:
    digitalWrite(BLUE, HIGH);
    digitalWrite(matrizPinodosLEDS[2],HIGH);     
    delay(200);     
    mediaSensor(5);                       
    matrizCores[2] = mediaLeituras;    
    Serial.print("Azul: "); 
    Serial.print(matrizCores[2]);
    Serial.print("\n");         
    cinzaDif = matrizBranco[2] - matrizPreto[2];
    Serial.print("CinzaDif[2]: "); 
    Serial.print(cinzaDif);
    Serial.print("\n"); 
    matrizCores[2] = (matrizCores[2] - matrizPreto[2])/(cinzaDif)*255;      
    digitalWrite(matrizPinodosLEDS[2],LOW);   
    digitalWrite(BLUE, LOW);    
 
 
    //passa os valores de r, g e b percebidos para a função que escreve no lcd qual a cor
    mostraCores(matrizCores[0],matrizCores[1],matrizCores[2]);
} 
 
void mediaSensor(int vezes){   
  int leituras;   
  int somatorio=0;   
  for(int i = 0;i < vezes;i++){     
    leituras = analogRead(LDR);   
    somatorio = leituras + somatorio;     
    delay(10);   
  }   
  mediaLeituras = (somatorio)/vezes; 
  Serial.print(mediaLeituras);
  Serial.print("\n"); 
}
 
 
//Checa a cor 
int checaCoresParaAndar(){ 
 
    //testa com o led vermelho: 
    digitalWrite(RED, HIGH);
    digitalWrite(matrizPinodosLEDS[0],HIGH);     
    delay(200);     
    mediaSensor(5);                       
    matrizCores[0] = mediaLeituras;   
    Serial.print("Vermelho: "); 
    Serial.print(matrizCores[0]);
    Serial.print("\n");          
    float cinzaDif = matrizBranco[0] - matrizPreto[0];  
    /*Serial.print("CinzaDif[0]: "); 
    Serial.print(cinzaDif);
    Serial.print("\n");    */
    matrizCores[0] = (matrizCores[0] - matrizPreto[0])/(cinzaDif)*255;      
    digitalWrite(matrizPinodosLEDS[0],LOW);   
    digitalWrite(RED, LOW);  
 
    //testa com o led verde:  
    digitalWrite(GREEN, HIGH);
    digitalWrite(matrizPinodosLEDS[1],HIGH);     
    delay(200);     
    mediaSensor(5);                       
    matrizCores[1] = mediaLeituras; 
    Serial.print("Verde: "); 
    Serial.print(matrizCores[1]);
    Serial.print("\n");             
    cinzaDif = matrizBranco[1] - matrizPreto[1];   
    Serial.print("CinzaDif[1]: "); 
    Serial.print(cinzaDif);
    Serial.print("\n");   
    matrizCores[1] = (matrizCores[1] - matrizPreto[1])/(cinzaDif)*255;      
    digitalWrite(matrizPinodosLEDS[1],LOW);   
    digitalWrite(GREEN, LOW);   
 
    //testa com o led azul:
    digitalWrite(BLUE, HIGH);
    digitalWrite(matrizPinodosLEDS[2],HIGH);     
    delay(200);     
    mediaSensor(5);                       
    matrizCores[2] = mediaLeituras;    
    Serial.print("Azul: "); 
    Serial.print(matrizCores[2]);
    Serial.print("\n");         
    cinzaDif = matrizBranco[2] - matrizPreto[2];
    Serial.print("CinzaDif[2]: "); 
    Serial.print(cinzaDif);
    Serial.print("\n"); 
    matrizCores[2] = (matrizCores[2] - matrizPreto[2])/(cinzaDif)*255;      
    digitalWrite(matrizPinodosLEDS[2],LOW);   
    digitalWrite(BLUE, LOW);    
 
 
    //passa os valores de r, g e b percebidos para a função que escreve no lcd qual a cor
    return mostraCoresParaAndar(matrizCores[0],matrizCores[1],matrizCores[2]);
} 
 
 
int mostraCoresParaAndar(int r, int g, int b){  
   if (r < 30 && g < 30 && b < 30){   
       lcd.setCursor(0,0);  
       lcd.print("Preto           ");  
       lcd.setCursor(0,1);  
       lcd.print("               ");   
       return 1;
    }
    else if((30<=r<=600)&&(r-20>g)&&(r-20>b)){
       lcd.setCursor(0,0);  
       lcd.print("Vermelho        ");  
       lcd.setCursor(0,1);  
       lcd.print("               ");   
       return 2;
    }
      else if((r>200)&& (g>200) &&(r>b)&&(g>b)){
       lcd.setCursor(0,0);  
       lcd.print("Amarelo         ");  
       lcd.setCursor(0,1);  
       lcd.print("               ");  
       return 3; 
    }
    else if((20<=g<=600)&&(g-20>r)&&(g-20>b)){
       lcd.setCursor(0,0);  
       lcd.print("Verde           ");  
       lcd.setCursor(0,1);  
       lcd.print("               ");   
       return 4;
    }
    else if((20<=b<=700)&&(b-20>r)&&(b-20>g)){
       lcd.setCursor(0,0);  
       lcd.print("Azul            ");  
       lcd.setCursor(0,1);  
       lcd.print("               ");   
       return 5;
    }
    return 0;
 
}
 
void mostraCores(int r, int g, int b){  
   if (r < 30 && g < 30 && b < 30){   
       lcd.setCursor(0,0);  
       lcd.print("Preto           ");  
       lcd.setCursor(0,1);  
       lcd.print("               ");   
    }
    else if((30<=r<=600)&&(r-20>g)&&(r-20>b)){
       lcd.setCursor(0,0);  
       lcd.print("Vermelho        ");  
       lcd.setCursor(0,1);  
       lcd.print("               ");   
    }
      else if((r>200)&& (g>200) &&(r>b)&&(g>b)){
       lcd.setCursor(0,0);  
       lcd.print("Amarelo         ");  
       lcd.setCursor(0,1);  
       lcd.print("               ");   
    }
    else if((20<=g<=600)&&(g-20>r)&&(g-20>b)){
       lcd.setCursor(0,0);  
       lcd.print("Verde           ");  
       lcd.setCursor(0,1);  
       lcd.print("               ");   
    }
    else if((20<=b<=700)&&(b-20>r)&&(b-20>g)){
       lcd.setCursor(0,0);  
       lcd.print("Azul            ");  
       lcd.setCursor(0,1);  
       lcd.print("               ");   
    }
 
}
cursos/introrobotica/2019-1/grupo04/codigo_tp2.txt · Última modificação: por 127.0.0.1
Donate Powered by PHP Valid HTML5 Valid CSS Driven by DokuWiki