Programación

//#include <EEPROM.h

// Buscanos en Facebook como Sleep & Listen
//------------------- Sumobot -----------------

// ------------------------------- Definiciones
#define      cPisoDer      A2  // Canal de piso derecha
#define      cPisoIzq      A3  // Canal de piso izquierda
#define      cDistIzq      A1  // Canal de distancia izquierda
#define      pista         0
#define      linea         1
#define      DerA          10
#define      DerB          11
#define      IzqA          9
#define      IzqB          6
#define      rFrente        'F'
#define      rAtras         'A'
#define      rDerecha       'D'
#define      rIzquierda     'I'
#define      memRutina      0



// ------------------------------- Varibales
int pisoDer;
int pisoIzq;
float distIzq;
int negroBlanco = 400;    // Este valor define si es linea o pista
int pwmDer = 255;
int pwmIzq = 255;
int error;
char rutina;


// ------------------------------- Configuraciones
void setup()
{
  Serial.begin(9600);
  pinMode(DerA,OUTPUT);
  pinMode(DerB,OUTPUT);
  pinMode(IzqA,OUTPUT);
  pinMode(IzqB,OUTPUT);
  detener();
  primerRutina();
}

// ------------------------------- Principal
void loop()
{
  leerPiso();
  while((pisoDer == pista) && (pisoIzq == pista)) // Mientras ambos esten en la pista
  {
    pelear();
    leerPiso();
    //Serial.println();
  }
  defender();
  //Serial.println();
}


// ------------- FUNCIONES --------------------
// -------------------------------- defender
void defender()
{
  //Serial.print("Defender");
  if((pisoDer == linea) && (pisoIzq == pista))
  {
    atras();
    delay(300);
    izquierda();
    delay(130);
  }
  else if((pisoIzq == linea) && (pisoDer == pista))
  {
    atras();
    delay(300);
    derecha();
    delay(130);
  }
  else
  {
    atras();
    delay(300);
  }
}


// -------------------------------- pelear
void pelear()
{
  //Serial.print("Pelear");
  leerDist();
  //adelante();
 
 
    error = -distIzq;
   // si el error es negativo gira a la izquierda
   if(error < -5)
   {
     pwmDer = 255;
     pwmIzq = 0; //velocidades motores
   }
   else if(error > 5)
   {
     pwmDer = 0;
     pwmIzq = 255;
   }
   else
   {
     pwmDer = 255;
     pwmIzq = 255;// (:
   }
   adelante();
 
    
}
// -------------------------------- primerRutina
void primerRutina()
{
  //Serial.print("PrimerRutina ");
  if(rutina == rDerecha)
  {
    derecha();
    delay(50);
  }
  else if(rutina == rIzquierda)
  {
    izquierda();
    delay(50);
  }
  else if(rutina == rAtras)
  {
    derecha();
    delay(100);
  }
 
}

// -------------------------------- adelante
void adelante()
{
  digitalWrite(DerA,HIGH);
  digitalWrite(DerB,LOW);
  digitalWrite(IzqA,HIGH);
  digitalWrite(IzqB,LOW);
 
  analogWrite(DerA,pwmDer);
  digitalWrite(DerB,LOW);
  analogWrite(IzqA,pwmIzq);
  digitalWrite(IzqB,LOW);
}

// -------------------------------- derecha
void derecha()
{
  //Serial.print("Derecha");
  digitalWrite(DerA,LOW);
  digitalWrite(DerB,HIGH);
  digitalWrite(IzqA,HIGH);
  digitalWrite(IzqB,LOW);
}

// -------------------------------- izquierda
void izquierda()
{
  //Serial.print("Izquierda");
  digitalWrite(DerA,HIGH);
  digitalWrite(DerB,LOW);
  digitalWrite(IzqA,LOW);
  digitalWrite(IzqB,HIGH);
}

// -------------------------------- atras
void atras()
{
  //Serial.print("Atras");
  digitalWrite(DerA,LOW);
  digitalWrite(DerB,HIGH);
  digitalWrite(IzqA,LOW);
  digitalWrite(IzqB,HIGH);
}

// -------------------------------- detenido
void detener()
{
  //Serial.print("Detenido");
  digitalWrite(DerA,LOW);
  digitalWrite(DerB,LOW);
  digitalWrite(IzqA,LOW);
  digitalWrite(IzqB,LOW);
}

// -------------------------------- Leer_dist
void leerDist()
{
 // Leer Sensor de piso y se convierte a centimetros
  distIzq = analogRead(cDistIzq)*0.0048828125;

  distIzq = 30*pow(distIzq, -1.10);
 
  //Serial.print("\t");
  //Serial.print(distDer);
  //Serial.print("\t");
  //Serial.print(distIzq);
  //Serial.print("\t");
}


// -------------------------------- Leer_Piso
void leerPiso()
{
  // Leer Sensores de piso y los binariza
  // entre pista y linea
 // Serial.print("\t");
  pisoDer = binSensor(analogRead(cPisoDer));
 // Serial.print("\t");
  pisoIzq = binSensor(analogRead(cPisoIzq));
  //Serial.print("\t");
 
}

// ----------------------- binSensor
// ayuda a diferenciar entre pista y linea
int binSensor(int sen)
{
  if(sen < negroBlanco)  // arriba de este valor se distingue linea
  {
    //Serial.print("linea");
    return linea;
  }
  else
  {
   // Serial.print("pista");
    return  pista;
  }
}

Socializer Widget By Blogger Yard

SOCIALIZE IT →

Tweet

FOLLOW US →

SHARE IT →