ROBOT SUMO CON SERVO MOTORES

 

CODIGO ARDUINO

#include <GyverOLED.h>

GyverOLED<SSD1306_128x32, OLED_BUFFER> oled;

#define sensorIzquierdo 2

#define sensorDerecho 3

#define boton A1 //pin 4 y pin negativo

#define motorIzquierdo 5

#define motorDerecho 6

#define echo 16     //Sensor ultrasonidos pin A2

#define trigger 17  //Sensor ultrasonidos pin A3

#define distanciaMaxima 40        // Esta será la distancia de detección del robot

//ANTES DE ARRANCAR EL PROGRAMA, CARGAMOS LAS SIGUIENTES LIBRERIAS PARA FACILITAR LA LABOR

#include <Servo.h>

#include <NewPing.h>

//ESTE CODIGO ES PARA INICIALIZAR LAS LIBRERIAS

Servo servoDerecho;   //Se inicia un servo con el nombre servoDerecho

Servo servoIzquierdo; //Se inicia un servo con el nombre servoIzquierdo

NewPing sonar(trigger, echo, distanciaMaxima); //Se inicia el sensor de ultrasonidos

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

boolean modoCombate = false;  // Inicializamos la variable para que el robot comience parado

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

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

int cm;           //Variable para medir la distancia con el sensor de ultrasonidos

unsigned int uS;  //Variable para medir la distancia con el sensor de ultrasonidos

//SETUP FUNCIONA UNA SOLA VEZ AL INICIO DEL PROGRAMA

//LA USAREMOS PARA CONFIGURAR LA FUNCION DE CADA PIN

void setup() {

  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

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

  pinMode(trigger, OUTPUT); //El pin enviara señales ultrasónicas, será una salida.

 

  servoDerecho.attach(motorDerecho);      //El pin va a manejar un servo

  servoIzquierdo.attach(motorIzquierdo);  //El pin va a manejar un servo

  //Al conectar el robot al ordenador por USB recibimos mensajes de diagnóstico

  Serial.begin(9600);

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

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

  oled.init();

 

  oled.clear();

  oled.setScale(2);

  oled.setCursor(6, 0);

  oled.print("Presione");

  oled.setCursor(6, 2);

  oled.print("el boton");

  oled.update();

}

//LA FUNCION LOOP SE REPITE CONSTANTEMENTE, ES EL CORAZON DEL PROGRAMA

void loop() {

  delay(50);      //Hacemos una pequeña pausa para estabilizar el programa

  leerBoton();    //Leemos el estado del boton, para arrancar o parar el robot

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

  if (modoCombate == false) //EL ROBOT ESTA EN ESPERA

    Parado();

  else { //EL ROBOT ESTA EN MODO COMBATE

    if (estadoSensorDerecho == LOW || estadoSensorIzquierdo == LOW)//DETECTA LA LINEA

      Evasion();  //Esquiva la linea

    if (cm != 0)  //DETECTA UN ENEMIGO

      Adelante(); //Ataca moviendose hacia delante

    else

      GirarDerecha();//BUSCA AL ENEMIGO

  }

}

//ESTA FUNCION LEE EL BOTON DEL ROBOT

void leerBoton() {

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

  if (estadoBoton == HIGH) {

    modoCombate = !modoCombate; //CAMBIAMOS EL ESTADO DEL ROBOT AL PULSAR EL BOTON

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

    if (modoCombate == true){ //ANTES DE ENTRAR EN COMBATE ESPERAMOS 5 SEGUNDOS

      Serial.println("ENTRANDO EN MODO COMBATE");

      oled.clear();

      oled.setScale(1);

      oled.setCursor(6, 0);

      oled.print("Iniciando combate en");

      oled.setCursor(6, 2);

      oled.print("5 segundos");

      oled.update();

      delay(4500);

    }

    if(modoCombate == false){

      Serial.println("ENTRANDO EN MODO REPOSO");

    }

  }

}

//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

  //Lee el sensor de ultrasonidos y convierte la informacion a centimetros para poder medir la distancia

  uS = sonar.ping();

  cm = sonar.convert_cm(uS);

  oled.clear();

  oled.setScale(2);

  oled.setCursor(6, 0);

  oled.print("Leyendo");

  oled.setCursor(6, 2);

  oled.print("Sensores");

  oled.update();

}

//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();

  servoDerecho.write(120);

  servoIzquierdo.write(120);

  delay(50);

}

void GirarIzquierda() {

  servoDerecho.write(0);

  servoIzquierdo.write(0);

  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();

  servoDerecho.write(0);

  servoIzquierdo.write(180);

  delay(100);

}

void Atras() {

  servoDerecho.write(180);

  servoIzquierdo.write(0);

  delay(50);

}

void Parado() {

  servoDerecho.write(90);

  servoIzquierdo.write(90);

  oled.clear();

  oled.setScale(1);

  oled.setCursor(6, 0);

  oled.print("Presione el boton");

  oled.setCursor(6, 2);

  oled.print("Para iniciar");

  oled.update();

  delay(50);

}

void Evasion() {

  Parado();

  delay(100);

  Serial.println("Evasion");

  Atras();

  delay(1000);

}