int mreverza = 7; //AIN2

int mfrente=8;
int mgizquierda = 9; //BIN1
int mgderecha =10;
int arrancador=4;
int PWM_Derecho=5;//El pin 5 de arduino se conecta con el pin EnA del L298N
int PWM_Izquierdo=6;
int sensoriz=11;
int sensorde=12;
#include <Wire.h>
#include <MechaQMC5883.h>
#include "NewPing.h"
MechaQMC5883 qmc;
#define TRIGGER_PIN_1 2
#define ECHO_PIN_1 2
#define TRIGGER_PIN_2 3
#define ECHO_PIN_2 3
#define MAX_DISTANCE 400
NewPing sonar1(TRIGGER_PIN_1, ECHO_PIN_1, MAX_DISTANCE);
NewPing sonar2(TRIGGER_PIN_2, ECHO_PIN_2, MAX_DISTANCE);

float duration1; // Stores First HC-SR04 pulse duration value

float duration2; // Stores Second HC-SR04 pulse duration value
float distance1; // Stores calculated distance in cm for First Sensor
float distance2; // Stores calculated distance in cm for Second Sensor
float soundsp; // Stores calculated speed of sound in M/S
float soundcm; // Stores calculated speed of sound in cm/ms
int iterations = 5;
int estado=0;
void setup() {
pinMode(mfrente,OUTPUT);
pinMode(mreverza,OUTPUT);
pinMode(mgderecha,OUTPUT);
pinMode(mgizquierda,OUTPUT);
pinMode(arrancador,OUTPUT);
pinMode(PWM_Derecho, OUTPUT);
pinMode(PWM_Izquierdo, OUTPUT);
pinMode(sensoriz,INPUT);
pinMode(sensorde,INPUT);

analogWrite(PWM_Derecho,80);//81 pwm original prueba 41

analogWrite(PWM_Izquierdo,180);

Serial.begin(9600);
digitalWrite(arrancador,LOW);
Wire.begin();
qmc.init();

void loop() {

soundsp=331.4;
soundcm = soundsp / 10000;
duration1 = sonar1.ping_median(iterations);
// delay(100);
duration2 = sonar2.ping_median(iterations);
distance1 = (duration1 /1.86) * soundcm;
distance2 = (duration2 /1.86) * soundcm;
 /* Serial.print("Distance 1: ");
Serial.print(distance1);
Serial.print(" cm ");
Serial.print("Distance 2: ");
Serial.print(distance2);
Serial.print(" cm");
Serial.println(" ");*/

////////////////estacionarce///////////////////////////////////////////////////////
//////////

if (distance2 <= 12) {

digitalWrite(mfrente,LOW);
digitalWrite(mreverza,LOW);
digitalWrite(mgderecha,LOW);
digitalWrite(mgizquierda,LOW);

digitalWrite(mfrente,HIGH);
digitalWrite(mgderecha,HIGH);
delay(150);
digitalWrite(mgderecha,LOW);
digitalWrite(mgizquierda,HIGH);
}
if (distance1 <= 12) {
digitalWrite(arrancador,LOW);
digitalWrite(mfrente,LOW);
digitalWrite(mreverza,LOW);
digitalWrite(mgderecha,LOW);
digitalWrite(mgizquierda,LOW);
}

if(digitalRead(sensorde)==LOW){

digitalWrite(arrancador,HIGH);
digitalWrite(mfrente,LOW);
digitalWrite(mreverza,HIGH);
digitalWrite(mgderecha,HIGH);
digitalWrite(mgizquierda,LOW);
delay(900);
digitalWrite(mreverza,HIGH);
digitalWrite(mgderecha,LOW);
digitalWrite(mgizquierda,HIGH);
}

/////////////////////bluethoot/////////////////////////////////////////////////////
///////
if( Serial.available()>0)
{
estado = Serial.read();
}
switch( estado)
{
case '1':
digitalWrite(arrancador,HIGH);
digitalWrite(mfrente,LOW);
digitalWrite(mreverza,LOW);
digitalWrite(mgderecha,LOW);
digitalWrite(mgizquierda,LOW);
break;
case '2':
digitalWrite(arrancador,LOW);
break;
case '3':
digitalWrite(mfrente,LOW);
digitalWrite(mreverza,LOW);
digitalWrite(mgderecha,LOW);
digitalWrite(mgizquierda,LOW);
break;
case 'A':
digitalWrite(mfrente,HIGH);
digitalWrite(mreverza,LOW);
digitalWrite(mgderecha,LOW);
digitalWrite(mgizquierda,LOW);
break;
case 'B':
digitalWrite(mfrente,LOW);
digitalWrite(mreverza,HIGH);
digitalWrite(mgderecha,LOW);
digitalWrite(mgizquierda,LOW);
break;
case 'C':
digitalWrite(mfrente,HIGH);
digitalWrite(mreverza,LOW);
digitalWrite(mgderecha,HIGH);
digitalWrite(mgizquierda,LOW);
break;
case 'D':
digitalWrite(mfrente,HIGH);
digitalWrite(mreverza,LOW);
digitalWrite(mgderecha,LOW);
digitalWrite(mgizquierda,HIGH);
break;
case 'E':
digitalWrite(mfrente,LOW);
digitalWrite(mreverza,HIGH);
digitalWrite(mgderecha,HIGH);
digitalWrite(mgizquierda,LOW);
break;
case 'F':
digitalWrite(mfrente,LOW);
digitalWrite(mreverza,HIGH);
digitalWrite(mgderecha,LOW);
digitalWrite(mgizquierda,HIGH);
break;
}
}