int motB0 = 2;
int motB1 = 3; //PWM
int motA1 = 4;
int motA0 = 5; //PWM
int lec0=A2;
int lec1=A1;
int sensorleft = A7;
int sensorcenter = A6;
int sensorright = A5;
long cont1 = 0;
int l1 = 0;
int l2 = 0;
int l3 = 0;
int f1 = 0;
int f2 = 0;
int f3 = 0;
int f4 = 0;
int f5 = 0;
int f6 = 0;
int buzzer = 6; //El pin al que se conecta el buzzer es el 6
int echo = 8;
int triger = 7;
int vel = 200;
char valorBoton;
char penultimo = 'n';
char antepenultimo = 'm';
char vali = 'b';
void setup()
{
Serial.begin(9600);
pinMode(motA0, OUTPUT);
pinMode(motA1, OUTPUT);
pinMode(motB0, OUTPUT);
pinMode(motB1, OUTPUT);
pinMode(buzzer, OUTPUT); // Pin 6 declarado como salida
pinMode(echo, INPUT);
pinMode(triger, OUTPUT);
digitalWrite(triger, LOW);
digitalWrite(motA0, LOW);
digitalWrite(motA1, LOW);
digitalWrite(motB0, LOW);
digitalWrite(motB1, LOW);
pinMode(sensorright, INPUT);
pinMode(sensorcenter, INPUT);
pinMode(sensorleft, INPUT);
pinMode(lec0,INPUT_PULLUP);
pinMode(lec1,INPUT_PULLUP);
}
void loop()
{
l3=analogRead(sensorleft);
l2=analogRead(sensorcenter);
l1=analogRead(sensorright);
if(digitalRead(lec0)==0){
if(digitalRead(lec1)==0){
// Seguidor de linea blanca
if ((l1 > 550 && l2 > 550 && l3 > 550 || l1 < 550 && l2 < 550 && l3 < 550 ) && cont1 > 0)
{
l1 = f1;
l2 = f2;
l3 = f3;
cont1=cont1-1;
}
if (l1 > 550 && l2 < 550 && l3 < 550 || l1 > 550 && l2 > 550 && l3 < 550)
{
derecha();
}
else if (l1 < 550 && l2 < 550 && l3 > 550 || l1 < 550 && l2 > 550 && l3 > 550)
{
izquierda();
}
else if (l1 < 550 && l2 > 550 && l3 < 550)
{
adelanter();
cont1 = 20000;
}
else
{
if(l1 > 550 && l2 > 550 && l3 > 550 || l1 < 550 && l2 < 550 && l3 < 550)
{
alto();
}
}
f1 = l1;
f2 = l2;
f3 = l3;
} else {
// A1=0 A2=1 SUMO
// Para volverlo sumo para fondo blanco cambiar los valores del if de la siguiente manera
// l1 > 550 && l2 > 550 && l3 > 550
if(distancia()<150){
if(l1 > 550 || l2 > 550 || l3 > 550)
{
alto();
delay(1000);
atras();
delay(1000);
derecha();
delay(700);
}
else{
adelanter();
}
}
else
{
if(l1 > 550 || l2 > 550 || l3 > 550)
{
alto();
delay(1000);
atras();
delay(1000);
derecha();
delay(700);
}
else{
adelantes();
}
}
}
} else {
if(digitalRead(lec1)==0){
// A1=1 A2=0 Evasor
if(distancia()<15){
alto();
delay(500);
atras();
delay(700);
derecha();
delay(500);
adelanter();
}
else {
adelanter();
}
} else {
// A1=1 A2=1 RC
if(Serial.available() > 0)
{
valorBoton = Serial.read();
if((antepenultimo == 'w' && penultimo == 'd' && valorBoton == 'h')
|| (antepenultimo == 'w' && penultimo == 'a' && valorBoton == 'f'))
{
vali = 'i';
adelanter();
penultimo = 'n';
antepenultimo = 'm';
}
else if((antepenultimo == 's' && penultimo == 'd' && valorBoton == 'h')
|| (antepenultimo == 's' && penultimo == 'a' && valorBoton == 'f'))
{
vali = 'k';
atras();
penultimo = 'n';
antepenultimo = 'm';
}
else if((antepenultimo == 'a' && penultimo == 'w' && valorBoton == 't')
|| (antepenultimo == 'a' && penultimo == 's' && valorBoton == 'g'))
{
vali = 'j';
izquierda();
penultimo = 'n';
antepenultimo = 'm';
}
else if((antepenultimo == 'd' && penultimo == 'w' && valorBoton == 't')
|| (antepenultimo == 'd' && penultimo == 's' && valorBoton == 'g'))
{
vali = 'l';
derecha();
penultimo = 'n';
antepenultimo = 'm';
}
else if(vali == 'i')
{
adelanter();
if(valorBoton == 't')
{
vali = 'b';
alto();
}
else if(valorBoton == 'd') {
derecha();
}
else if(valorBoton == 'a') {
izquierda();
}
}
else if(vali == 'k')
{
atras();
if(valorBoton == 'g')
{
vali = 'b';
alto();
}
else if(valorBoton == 'd') {
atrasderecha();
}
else if(valorBoton == 'a') {
atrasizquierda();
}
}
else if(vali == 'j')
{
izquierda();
if(valorBoton == 'f')
{
vali = 'b';
alto();
}
else if(valorBoton == 'w') {
izquierda();
}
else if(valorBoton == 'a') {
atrasderecha();
}
}
else if(vali == 'l')
{
derecha();
if(valorBoton == 'h')
{
vali = 'b';
alto();
}
else if(valorBoton == 'w') {
derecha();
}
else if(valorBoton == 'a') {
atrasizquierda();
}
}
else if(valorBoton == 'w') {
adelanter();
}
else if(valorBoton == 's') {
atras();
}
else if(penultimo != 's' && valorBoton == 'a') {
izquierda();
}
else if(penultimo != 's' && valorBoton == 'd') {
derecha();
}
else if(penultimo == 'w' && valorBoton == 'a') {
izquierda();
}
else if(penultimo == 'w' && valorBoton == 'd') {
derecha();
}
else if(penultimo == 'w' && valorBoton == 'f') {
adelanter();
}
else if(penultimo == 'w' && valorBoton == 'h') {
adelanter();
}
else if(penultimo == 't' && valorBoton == 'a') {
izquierda();
}
else if(penultimo == 't' && valorBoton == 'd') {
derecha();
}
else if(penultimo == 's' && valorBoton == 'a') {
atrasizquierda();
}
else if(penultimo == 's' && valorBoton == 'd') {
atrasderecha();
}
else if(penultimo == 's' && valorBoton == 'f') {
atras();
}
else if(penultimo == 's' && valorBoton == 'h') {
atras();
}
else if(penultimo == 'g' && valorBoton == 'a') {
atrasderecha();
}
else if(penultimo == 'g' && valorBoton == 'd') {
atrasizquierda();
}
else if(penultimo == 'a' && valorBoton == 'w') {
izquierda();
}
else if(penultimo == 'd' && valorBoton == 'w') {
derecha();
}
else if(penultimo == 'f' && valorBoton == 'w') {
adelanter();
}
else if(penultimo == 'h' && valorBoton == 'w') {
adelanter();
}
else if(penultimo == 'a' && valorBoton == 't') {
izquierda();
}
else if(penultimo == 'd' && valorBoton == 't') {
derecha();
}
else if(penultimo == 'a' && valorBoton == 's') {
atrasderecha();
}
else if(penultimo == 'd' && valorBoton == 's') {
atrasizquierda();
}
else if(penultimo == 'f' && valorBoton == 's') {
atras();
}
else if(penultimo == 'h' && valorBoton == 's') {
atras();
}
else if(penultimo == 'a' && valorBoton == 'g') {
atrasderecha();
}
else if(penultimo == 'd' && valorBoton == 'g') {
atrasizquierda();
}
else {
alto();
penultimo = 'n';
antepenultimo = 'm';
}
Serial.print(antepenultimo);
Serial.print(" ");
Serial.print(penultimo);
Serial.print(" ");
Serial.print(valorBoton);
Serial.print(" ");
Serial.print(vali);
Serial.println(" ");
antepenultimo = penultimo;
penultimo = valorBoton;
}
}
}
}
long distancia()
{
long t;
long d;
digitalWrite(triger, HIGH);
delayMicroseconds(10);
digitalWrite(triger, LOW);
t = pulseIn(echo, HIGH);
d = t / 59;
if (d <= 1 || d >= 25)
{
d = 1000;
}
delay(10);
return d;
}
void alto()
{
digitalWrite(motA0, 0);
digitalWrite(motA1, 0);//PWM
digitalWrite(motB0, 0); //PWM
digitalWrite(motB1, 0);
}
void atras()
{
digitalWrite(motA0, 1);
digitalWrite(motA1, 0); //PWM
digitalWrite(motB1, 1); //PWM
digitalWrite(motB0, 0);
}
void adelante()
{
digitalWrite(motA1, 1);
analogWrite(motA0, 50); //PWM
analogWrite(motB1, 50); //PWM
digitalWrite(motB0, 1);
}
void adelanter()
{
digitalWrite(motA1, 1);
digitalWrite(motA0, 0); //PWM
digitalWrite(motB0, 1); //PWM
digitalWrite(motB1, 0);
}
void adelantes()
{
digitalWrite(motA1, 1);
analogWrite(motA0, 80); //PWM
digitalWrite(motB0, 1); //PWM
digitalWrite(motB1, 0);
}
void izquierda()
{
digitalWrite(motA0, 0);
digitalWrite(motA1, 1); //PWM
digitalWrite(motB0, 0); //PWM
digitalWrite(motB1, 0);
}
void derecha()
{
digitalWrite(motA1,0);
digitalWrite(motA0, 0); //PWM
digitalWrite(motB0, 1); //PWM
digitalWrite(motB1, 0);
}
void atrasderecha(){
digitalWrite(motA0, 0);
digitalWrite(motA1, 0); //PWM
digitalWrite(motB0, 0); //PWM
digitalWrite(motB1, 1);
}
void atrasizquierda(){
digitalWrite(motA0, 1);
digitalWrite(motA1, 0); //PWM
digitalWrite(motB0, 0); //PWM
digitalWrite(motB1, 0);
}
