ArduSoccerBot

Programa principal del ArduSoccerBot versión 1.0

//  ArduSoccerBot
//
//  Localiza y sigue una pelota emisora de infrarrojos
//
//  Nerea de la Riva, Octubre 2008 Ver.:1.0

#include 

#define KP 5    // Constante de proporcionalidad			 
#define V_MIN 50

#define POT 5  // Entrada analógica 5 para el potenciómetro de umbral

//Declaración de funciones
void espera_go_pulsado();
void mueve_motores(int vel_a, int vel_b);

//Entradas y salidas
int sensor_izquierdo=0;  // Fototransistor izquierdo
int sensor_derecho= 1;   // Fototransistor derecho
int go=11;                // Pulsador de "GO"
int ledPin = 13;         // Led de "Veo pelota"

//Variables del programa		 
int si=0;          // Valor del sensor izquierdo
int sd=0;          // Valor del sensor derecho
int diferencia=0;  // Diferencia entre le sensor izquierdo y el derecho
int umbral=0;      // Nivel de luz mínimo para ver pelota

Servo motor_a;  // Servo de la rueda A
Servo motor_b;  // Servo de la rueda B

//inicialización
void setup()  
{
  // Inicializa servos
  ;motor_a.attach(9);   // Asocia el servo del motor A a la salida PWM 9
  ;motor_b.attach(10);  // Asocia el servo del motor B a la salida PWM 10
  
  // Inicializa entradas/salidas
  pinMode(go, INPUT);
  pinMode(ledPin, OUTPUT);
  
  // Detiene el robot
  mueve_motores(0,0);
  
  // Espera a que se pulse GO para comenzar el programa
  espera_go_pulsado();
}				 

// Programa principal
void loop()
{
  si = analogRead(sensor_izquierdo);  //medimos el valor del sensor izquiero
  sd = analogRead(sensor_derecho);    //medimos el valor del sensor derecho 
  umbral = map (analogRead(POT),0,1023,0,50); //Usamos el valor del potenciómetro para fijar el Umbral 
  
  if((si > umbral) || (sd > umbral))      //si uno o los dos sensore ve la pelota
  {
    digitalWrite(ledPin, HIGH);        //Encendemos LED 
    diferencia = (si-sd)/KP;           //Calculamos la diferencia entre sensores
    
    //Miramos por que lado se encuentra la pelota
    if(diferencia>0)                  //Pelota hacia la izquierda
    {
      if (diferencia > V_MIN)    
        diferencia =V_MIN;
					 
      mueve_motores(100-diferencia,100);
    }
    else
    {
      if (diferencia < -V_MIN)            //pelota hacia la derecha

        diferencia = -V_MIN;
      mueve_motores(100,100 + diferencia);
    }
  }
  else                                  //si no ve pelota
  {
    digitalWrite(ledPin, LOW);           //Apagamos LED
    mueve_motores(30,-30);               //Modo búsqueda: giramos lentamente
  }
  	delay(10);
}

// Espera hasta que apretemos y soltemos el pulsador de GO
void espera_go_pulsado()
{
  while(digitalRead(go)==true);  // Espera que se pulse
  delay(100); // Espera anti rebotes
  while(digitalRead(go)==false); // Espera que se suelte
  delay(100); // Espera anti rebotes
}

// Mueve los motores en el rango de -100 a 100
void mueve_motores(int vel_a, int vel_b)
{
  motor_a.write(map(vel_a, -100, 100, 76, 116));
  motor_b.write(map(vel_b, 100, -100, 76, 116));
}