ROBOT MINI SUMO CON ARDUINO NANO Y DRIVER TB6612FNG

 ROBOT MINI SUMO

CODIGO ARDUINO

//FUNCIONAL 5 AGOSTO 2024

//AL COMIENZO DEL PROGRAMA DEFINIMOS EN QUE PINES IRAN CONECTADOS LOS DISTINTOS DISPOSITIVOS

//LINEA BLANCA = 0   ---- LINEA NEGRA = 1

#include <GyverOLED.h>

GyverOLED<SSD1306_128x32, OLED_BUFFER> oled;

#include <GP2Y0A21YK.h>

#define PIN_PSD (A2)

GP2Y0A21YK *psd;

#define sensorIzquierdo 11

#define sensorDerecho 10

#define boton 2 //pin 4 y pin negativo

#define STBY 3

#define Ain1 9

#define Ain2 8

#define Bin1 7

#define Bin2 4

#define PwmA 5

#define PwmB 6

double distance;

//DECLARAMOS UNA SERIE DE VARIABLES PARA HACER EL PROGRAMA MAS LEGIBLE Y PRÁCTICO

int estadoSensorDerecho;    //Variable que almacena el estado del sensor Derecho

int estadoSensorIzquierdo;  //Variable que almacena el estado del sensor Izquierdo

int modo = 1; // si pista fondo negro linea blanca usar 0 // si pista fondo blanco linea negra usar 1

int estadoBoton = 0;

int estadoreposo = 1; //Activado = 1 --- DESACTIVADO = 0

int estadocombate = 0;

void setup() {

  psd = new GP2Y0A21YK(PIN_PSD);

  pinMode(STBY  ,OUTPUT);

  pinMode(Ain1, OUTPUT);

  pinMode(Ain2, OUTPUT);

  pinMode(Bin1, OUTPUT);

  pinMode(Bin2, OUTPUT);

  pinMode(PwmA, OUTPUT);

  pinMode(PwmB, OUTPUT);

  pinMode(boton, INPUT); //El boton sera una entrada

  pinMode(sensorDerecho, INPUT);  //El pin sera una entrada para leer el sensor de linea

  pinMode(sensorIzquierdo, INPUT);//El pin sera una entrada para leer el sensor de linea

 

  analogWrite(PwmA, 140);

  analogWrite(PwmB, 140);

  digitalWrite(STBY, HIGH);

 

  Serial.begin(9600);

  oled.init();

  oled.setCursor(5, 0);   // курсор в (пиксель X, строка Y)

  oled.setScale(1);

  oled.print("ROBOT PREPARADO...");

  oled.setCursor(1, 1);

  oled.print("Presione el boton");

  oled.setCursor(10, 2);

  oled.print("para");

  oled.setCursor(1, 3);

  oled.print("Iniciar combate");

  oled.update();

  Serial.println("ROBOT PREPARADO...");

  Serial.print("ESTADO BOTON = ");

  Serial.println(estadoBoton);

  Serial.print("ESTADO COMBATE = ");

  Serial.println(estadocombate);

  Serial.print("ESTADO REPOSO = ");

  Serial.println(estadoreposo);

  Serial.println("Presione el boton para comenzar el combate");

}

void loop() {

   

    estadoBoton = digitalRead(boton);  //lee el estado del boton

      if((estadoBoton == 1)&&(estadocombate == 1)&&(estadoreposo == 0)){

      estadocombate = 0;

      estadoreposo = 1;

      delay(1000);

      }

   

    estadoBoton = digitalRead(boton);  //lee el estado del boton

      if((estadoBoton == 1)&&(estadocombate == 0)&&(estadoreposo == 1)){

      estadocombate = 1;

      estadoreposo = 0;

      Serial.println("Iniciando modo combate en 5 segundos");

      delay(5000); //delay antirebote, para que el boton no mande distintas señales

      Serial.println("MODO COMBATE INICIADO");

      }

    if((estadocombate == 1)&&(estadoreposo == 0)){

    //Serial.println("MODO COMBATE INICIADO");

    leerSensores(); //Leemos los sensores de ultrasonidos y de linea

   

    if(estadoSensorDerecho == modo || estadoSensorIzquierdo == modo)//DETECTA LA LINEA BLANCA = 0

      Evasion();  //Esquiva la linea

   

    if((distance <= 40)&&(estadoSensorDerecho == !modo)&&(estadoSensorIzquierdo == !modo)) //DETECTA UN ENEMIGO a menos de 40cm

      Adelante(); //Ataca moviendose hacia delante

   

    if((distance >= 41)&&(estadoSensorDerecho == !modo)&&(estadoSensorIzquierdo == !modo))

      GirarDerecha();//BUSCA AL ENEMIGO

   }

   

    //estadoBoton = digitalRead(boton);  //Debe volver a leer el nuevo estado del boton

      if((estadocombate == 0)&&(estadoreposo == 1)){

        Parado();

      //delay(1000);

      }

   

   

  /*

   estadoBoton = digitalRead(boton);

   if((estadoBoton == 1)&&(estadocombate == 0)&&(estadoreposo == 1)){

     Serial.println("MODO REPOSO ACTIVADO");

     Parado();

   }

*/

}

 

//ESTA FUNCION LEE LOS SENSORES DEL ROBOT

void leerSensores() {

  estadoSensorDerecho = digitalRead(sensorDerecho); //Lee el sensor de linea derecho

  estadoSensorIzquierdo = digitalRead(sensorIzquierdo); //Lee el sensor de linea izquierdo

  distance = psd->distance();

  }

//A PARTIR DE AQUI TODO SON MANIOBRAS DE MOVIMIENTO

void GirarDerecha() {

  Serial.println("Buscando Enemigo...");

  oled.clear();

  oled.setScale(2);

  oled.setCursor(6, 0);

  oled.print("Buscando");

  oled.setCursor(6, 2);

  oled.print("Enemigo");

  oled.update();

  digitalWrite(Ain1, HIGH);

  digitalWrite(Ain2, LOW);

  digitalWrite(Bin1, LOW);

  digitalWrite(Bin2, HIGH);

  //delay(50);

}

void GirarIzquierda() {

  digitalWrite(Ain1, LOW);

  digitalWrite(Ain2, HIGH);

  digitalWrite(Bin1, HIGH);

  digitalWrite(Bin2, LOW);

  Serial.println("GIRANDO A LA IZQUIERDA");

  //delay(50);

}

void Adelante() {

  Serial.println("ATACANDO");

  oled.clear();

  oled.setScale(2);

  oled.setCursor(6, 0);

  oled.print("Atacando");

  oled.setCursor(6, 2);

  oled.print(".....");

  oled.update();

  digitalWrite(Ain1, HIGH);

  digitalWrite(Ain2, LOW);

  digitalWrite(Bin1, HIGH);

  digitalWrite(Bin2, LOW);

  //delay(100);

}

void Atras() {

  digitalWrite(Ain1, LOW);

  digitalWrite(Ain2, HIGH);

  digitalWrite(Bin1, LOW);

  digitalWrite(Bin2, HIGH);

  //Serial.println("ATRAS");

  //delay(50);

}

void Parado() {

  estadocombate = 0;

  estadoreposo = 1;

  digitalWrite(Ain1, LOW);

  digitalWrite(Ain2, LOW);

  digitalWrite(Bin1, LOW);

  digitalWrite(Bin2, LOW);

  Serial.println("ROBOT PARADO");

  //delay(50);

}

void Evasion() {

  //Parado();

  //delay(100);

  Serial.println("Evasion");

  oled.clear();

  oled.setScale(2);

  oled.setCursor(6, 0);

  oled.print("Evasion");

  oled.setCursor(6, 2);

  oled.print("Perimetro");

  oled.update();

  Atras();

  //delay(1000);

}