miércoles, 6 de junio de 2012

Andromie - Prueba de Sensores

Los sensores de proximidad del robot Andromie, son de tipo digital, esto es que dan un 0 lógico si hay ausencia de un obstáculo o está fuera de sus alcance, y un 1 lógico si enfrente del sensor se encuentra un objeto, obstáculo.
Para graduar la distancia a la cual el sensor reconocerá el objeto y pondrá un 1 lógico a su salida, gire con un destornillador pequeño el preset que se encuentra en la tarjeta del mismo. Fíjese para que lado el rango de distancia del sensor es mayor y fíjelo.
Para hacer la prueba de los sensores ensamble la tarjeta "MotorShield" con el arduino y descargue el siguiente código.

/*
 Prueba de sensores de proximidad
 Andromie RF
 CIMecatrónica 2012
 */
 
//Asignación de pines para los motor derecho
int pwmtoder = 10;
int mtoder = 12;

//Asignación de pines para los motor izquierdo
int pwmtoizq = 11;
int mtoizq = 13;

//Asignación de pin de entrada para sensor central
int SenCen = 2;
int ValSenCen;

//Asignación de pin de entrada para sensor derecho
int SenDer = 3;
int ValSenDer;

//Asignación de pin de entrada para sensor izquierdo
int SenIzq = 4;
int ValSenIzq;

//Rutinas
void adelante();
void atras();
void giroderecha();
void giroizquierda();
void derecha();
void izquierda();
void detenido();

//Configuración de entradas y salidas
void setup() {
  pinMode(SenCen, INPUT);
  pinMode(SenDer, INPUT);
  pinMode(SenIzq, INPUT);
  
  pinMode(pwmtoizq, OUTPUT);     
  pinMode(mtoizq, OUTPUT);     
   
  pinMode(pwmtoder, OUTPUT);     
  pinMode(mtoder, OUTPUT);
  
  digitalWrite(pwmtoizq, HIGH);
  digitalWrite(mtoizq, LOW);
  
  digitalWrite(pwmtoder, HIGH);
  digitalWrite(mtoder, LOW);
}

//Ciclo principal
void loop() 
{
ValSenCen = digitalRead(SenCen); //Lectura digital del sensor de proximidad, el resultado se guarda en la variable
  
if(ValSenCen == HIGH) //Comparación, si la lectura es de un 1 lógico hay un objeto, entonces se detiene
{                     // retrocede y gira
    detenido();
    delay(1000);  
    atras();
    delay(500);
    giroderecha();
    delay(500);
}
else                  //Si la lectura del sensor es un 0 lógico entonces se sigue hacia adelante
  {
    adelante();
  }
}


void adelante()  //Función adelante
{
  digitalWrite(pwmtoder, HIGH);
  digitalWrite(pwmtoizq, HIGH);  
  digitalWrite(mtoder,   HIGH);   
  digitalWrite(mtoizq, HIGH);  
}

void atras()   //Función atrás
{
  digitalWrite(pwmtoder, HIGH);
  digitalWrite(pwmtoizq, HIGH); 
  digitalWrite(mtoder, LOW);   
  digitalWrite(mtoizq, LOW);    
}

void giroderecha()
{
   digitalWrite(pwmtoder, HIGH);
  digitalWrite(pwmtoizq, HIGH); 
  digitalWrite(mtoder, LOW);   
  digitalWrite(mtoizq, HIGH);   
}

void derecha()
{
  digitalWrite(pwmtoder, LOW);
  digitalWrite(pwmtoizq, HIGH); 
  digitalWrite(mtoder, LOW);   
  digitalWrite(mtoizq, HIGH);   
}

void giroizquierda()
{
  digitalWrite(pwmtoder, HIGH);
  digitalWrite(pwmtoizq, HIGH); 
  digitalWrite(mtoder, HIGH);   
  digitalWrite(mtoizq, LOW);   
}

void izquierda()
{
  digitalWrite(pwmtoder, HIGH);
  digitalWrite(pwmtoizq, LOW); 
  digitalWrite(mtoder, HIGH);   
  digitalWrite(mtoizq, LOW); 
}

void detenido()
{
  digitalWrite(pwmtoder, LOW);
  digitalWrite(pwmtoizq, LOW);
}

Con el programa anterior se hace la prueba del sensor central, y ocurre que cuando se detecta un objeto se detiene, retrocede  y gira el robot para evadirlo. Para hacer la prueba de los demás sensores haga la lectura del sensor derecho o izquierdo.