sumo

/******************************************************************************
  TestRun.ino
  TB6612FNG H-Bridge Motor Driver Example code
  Michelle @ SparkFun Electronics
  8/20/16
  https://github.com/sparkfun/SparkFun_TB6612FNG_Arduino_Library

  Uses 2 motors to show examples of the functions in the library.  This causes
  a robot to do a little 'jig'.  Each movement has an equal and opposite movement
  so assuming your motors are balanced the bot should end up at the same place it
  started.

  Resources:
  TB6612 SparkFun Library

  Development environment specifics:
  Developed on Arduino 1.6.4
  Developed with ROB-9457
******************************************************************************/

// This is the library for the TB6612 that contains the class Motor and all the
// functions
#include <SparkFun_TB6612.h>

// Pins for all inputs, keep in mind the PWM defines must be on PWM pins
// the default pins listed are the ones used on the Redbot (ROB-12097) with
// the exception of STBY which the Redbot controls with a physical switch
#define AIN1 7
#define AIN2 8

#define BIN1 2
#define BIN2 4

#define PWMA 6
#define PWMB 5

#define STBY 9

// these constants are used to allow you to make your motor configuration
// line up with function names like forward.  Value can be 1 or -1
const int offsetA = 1;
const int offsetB = 1;

// Initializing motors.  The library will allow you to initialize as many
// motors as you have memory for.  If you are using functions like forward
// that take 2 motors as arguements you can either write new functions or
// call the function more than once.
Motor motor1 = Motor(AIN1, AIN2, PWMA, offsetA, STBY);
Motor motor2 = Motor(BIN1, BIN2, PWMB, offsetB, STBY);


//Declaracion de pines para sensores
int SharpDer = A0;
int SharpIzq = A1;
int QtrDer   = A2;
int QtrIzq   = A3;
int SharpladoDer = A4;
int SharpladoIzq = A5;


//Declaracion de variables para leer los valores de los sensores
int Sizq = 0;
int Sder = 0;
int SLadoizq = 0;
int SLadoder = 0;
int Qder = 0;
int Qizq = 0;
void setup()
{
  //Nothing here
  Serial.begin(115200);

pinMode(SharpDer, INPUT);
pinMode(SharpIzq, INPUT);
pinMode(QtrDer, INPUT);
pinMode(QtrIzq, INPUT);
pinMode(SharpladoDer, INPUT);
pinMode(SharpladoIzq, INPUT);

  Serial.println("Goodnight moon!");

}

void loop() {


  sensores();
  // Estos valores de 300 pueden cambiar dependiendo de cada sensor, cuando es menor a 300 se refiere a que no detecta nada, y mayor a 300 es que el sensor esta detectando algo
  // sin embargo eso lo debes comprobar tu mismo con tus sensores.
  if ((SLadoizq <= 300) && (Sizq <= 300) && (Sder <= 300) && (SLadoder <= 300)) {
    derechasuave(); // 0 0 0 0=> elegir entre ir adelante o dar vueltas
  }
  if ((SLadoizq <= 300) && (Sizq <= 300) && (Sder <= 300) && (SLadoder >= 300)) {
    derechafuerte(); // 0 0 0 1
  }
  if ((SLadoizq <= 300) && (Sizq <= 300) && (Sder >= 300) && (SLadoder <= 300)) {
    derechasuave(); // 0 0 1 0
  }
  if ((SLadoizq <= 300) && (Sizq >= 300) && (Sder <= 300) && (SLadoder <= 300)) {
    izquierdasuave(); // 0 1 0 0
  }
  if ((SLadoizq <= 300) && (Sizq >= 300) && (Sder >= 300) && (SLadoder <= 300)) {
    adelante(); // 0 1 1 0
  }
  if ((SLadoizq >= 300) && (Sizq <= 300) && (Sder <= 300) && (SLadoder <= 300)) {
    izquierdafuerte(); // 1 0 0 0
  }

  //Para los sensores QTR los valores menores a 500 indican que detecto la liena blanca, por lo que si cualquiera de los 2 sensores detecta la linea blanca el robot debe regresar
  // Tu decides si quieres que solo regrese o tambien quieres que gire a la derecha o izquierda

  if ((Qder <= 500) || (Qizq <= 500)) {
    atras();  //el tiempo del delay depende de que tan rapido sea tu robot
    delay(200);
  }

}



// Lectura de Sensores
void sensores () {
  Sder = analogRead(SharpDer);
  Serial.println(SharpDer);

  delay(1);
  Sizq = analogRead(SharpIzq);
  Serial.println(SharpIzq);

  delay(1);
  SLadoder = analogRead(SharpladoDer);
  Serial.println(SharpladoDer);

  delay(1);
  SLadoizq = analogRead(SharpladoIzq);
  Serial.println(SharpladoIzq);

  delay(1);

  Qder = analogRead(QtrDer);
  Serial.println(QtrDer);

  delay(1);
  Qizq = analogRead(QtrIzq);
  Serial.println(QtrIzq);

  delay(1);
}


void adelante() {
  forward(motor1, motor2, 150);

}
void atras() {
  back(motor1, motor2, -150);

}
void parar() {
  brake(motor1, motor2);

}

void derechasuave() {
  right(motor1, motor2, 100);


}
void derechafuerte() {
  right(motor1, motor2, 300);

}

void izquierdasuave() {
  left(motor1, motor2, 100);

}
void izquierdafuerte() {
  left(motor1, motor2, 300);
}