Repetir acción sólo una vez - Establecer más de un condicional

International Español Software
4

Lo primero que veo en tu código es que tendrías que haber creado una máquina de estados donde estuviese mas claro que pasos esta dando el robot.

Haciendo lo mejor que pude con el poco tiempo que tienes ya que para mi gusto habría que rehacer todo, prueba este comportamiento del código

#include <Servo.h>      

int izqA = 5;
int izqB = 4;
int derA = 3;
int derB = 2;

int leda = 12;
int ledr = 13;

int Trig = 8;
int Echo = 9;
int Trig2 = 10;
int Echo2 = 11;

int servopin = 6;
int servoPin2 = 7; 

int vel = 200;           // Velocidad de los motores (0 - 200)
long distanciaizquierda = 0;
long distanciaderecha = 0;
long distanciafrente = 0;
     
long distancia;
long distancia2;
long distancia3;
long distancia4;

Servo Servo1;                     

Servo myservo;
enum modo {BUSCAR = 0, RECOGER}
modo = BUSCAR;


void setup ()
{
  Serial.begin(9600);
  pinMode(izqA,OUTPUT);  
  pinMode(izqB,OUTPUT);
  pinMode(derA,OUTPUT);
  pinMode(derB,OUTPUT);
  pinMode(ledr,OUTPUT);
  pinMode(leda,OUTPUT);
  pinMode(Trig,OUTPUT);
  pinMode(Echo,INPUT);
  pinMode(Trig2,OUTPUT);
  pinMode(Echo2,INPUT);
  myservo.attach(servopin);
  Servo1.attach(servoPin2); 
  myservo.write(90);
  delay(500);

}

void loop() {
  buscarruta();
  
  if (distancia <= 5) { 
     if (modo == RECOGER)
        recoger();
     else {
        detenerse();
        delay(5000); // pausa detenido en sitio donde no recoge objeto.
        modo = RECOGER;
     }
  }
}

void recoger() {
  detenerse();  
  distancia = leo_ultrasonico2();

  Serial.println("Distancia objeto =");
  Serial.println(distancia);
  Serial.println(" cm");

  
  delay(4000);             
  Servo1.write(-180);
  delay(2000);
  modo = BUSCAR;  // siguiente objeto no sera recogido.
}

void buscarruta() {                       
  detenerse();     
  busca_objeto();                      
  
  if ((distanciafrente < distanciaizquierda) && 
      (distanciafrente < distanciaderecha))  { 
      adelanta(); 
  }           
                    
  else 
  if ((distanciaizquierda < distanciaderecha) && 
      (distanciaizquierda < distanciafrente)) {
      giraizquierda();
      adelanta();
  }
  else {
      giraderecha ();
      adelanta();
  }
}


void adelanta() {                         //Se mueve hacia adelante.
  digitalWrite(leda, HIGH);
  digitalWrite(ledr, LOW);
  analogWrite(derA, 0);
  analogWrite(derB,vel);  
  analogWrite(izqA, 0);
  analogWrite(izqB,vel);
}

void retrocede() {                            
  digitalWrite(leda, LOW);
  digitalWrite(ledr, HIGH);
  analogWrite(izqA,vel);
  analogWrite(izqB,0);
  analogWrite(derA,vel);
  analogWrite(derB,0);
  delay(400);
  detenerse();
}

void detenerse() {                          
  digitalWrite(leda, LOW);
  digitalWrite(ledr, LOW); 
  analogWrite(izqA,0);
  analogWrite(izqB,0);
  analogWrite(derA,0);
  analogWrite(derB,0);
  delay(400);                
}

void busca_objeto() {
  miraizquierda();                        
  mirafrente();
  miraderecha();                          
}
 
void miraizquierda() {                      //Se configuran los pasos a seguir cuando mira a la izquierda.
  myservo.write(170);
  delay(2000);
   
  distanciaizquierda = leo_ultrasonico1();
  Serial.println("Distancia Izquierda =");
  Serial.println(distanciaizquierda);
  Serial.println(" cm");                                                       
}

void mirafrente() {
  myservo.write(90);
  delay(2000); 
   
  distanciafrente = leo_ultrasonico1();
  Serial.println("Distancia frente    =");
  Serial.println(distanciafrente);
  Serial.println(" cm");
                          
  myservo.write(10);
  delay(2000);                       
}

void miraderecha()  {                      //Se configuran los pasos a seguir cuando mira a la derecha.
    
  distanciaderecha = leo_ultrasonico1();

  Serial.println("Distancia Derecha   =");
  Serial.println(distanciaderecha);
  Serial.println(" cm");
                          
  myservo.write(90);                                  
  delay(2000);                       
}

void giraizquierda () {                   //Se configuran los pasos a seguir cuando gira a la izquierda.
  digitalWrite(leda, LOW);
  digitalWrite(ledr, HIGH);
  analogWrite(izqA,0);      
  analogWrite(izqB,vel);     
  analogWrite(derA,vel);
  analogWrite(derB,0);
  delay(2000);                    
  adelanta();
  delay(3000);
}

void giraderecha () {                    //Se configuran los pasos a seguir cuando gira a la derecha.
  digitalWrite(leda, LOW);
  digitalWrite(ledr, HIGH);
  analogWrite(izqA,vel);       
  analogWrite(izqB,0);   
  analogWrite(derA,0);
  analogWrite(derB,vel);
  delay(2000); 
  adelanta();
  delay(3000);
}

long leo_ultrasonico1() {
  int tmp;

  digitalWrite(Trig, LOW);
  delayMicroseconds(5);
  digitalWrite(Trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(Trig, LOW);

  tmp = pulseIn(Echo, HIGH)/2;
  return tmp;
}

long leo_ultrasonico2() {
  long tmp;

  digitalWrite(Trig2, LOW);
  delayMicroseconds(5);
  digitalWrite(Trig2, HIGH);
  delayMicroseconds(10);
  digitalWrite(Trig2, LOW);

  tmp = pulseIn(Echo2, HIGH)/2;
   
  return tmp/29;
}