Ayuda con codigo para robot con ultra sonido

International Español
1

hola como estan?, queria ver si me pueden ayudar con un aspecto de mi codigo, estoy armando un robot recoge basura con motores DC, servos y un sensor de ultra sonido, tengo este codigo armado que al estar a una distancia menor a 20cm ejecuta unas acciones para agarrar la basura con una pala y una escoba hechas en 3d, mi problema viene a que no se como hacer para si por ejemplo detecta basura a 100,110,120cm, etc se mueva hasta que llegue a 20cm y ejecute la accion, basicamente que no importa la distancia en la que este mientras lo detecte se mueva hasta llegar a los 20cm y mueva los servos
adjunto codigo:

#include <Servo.h>
#include <Ultrasonic.h>

Ultrasonic ultrasonic(6, 7); // (Trig PIN,Echo PIN)
Servo miniservo1;
Servo miniservo2;

int distancia;

unsigned long tiempo1 = 0;
unsigned long tiempo2 = 0;
unsigned long tiempoSegundos = 0;

bool flag = true;

int IN3 = 5;      // IN1 de L298N a pin digital 2
int IN4 = 4;      // IN2 de L298N a pin digital 3
int ENB = 3;      // ENA de L298N a pin digital 5

int IN2 = 8;      // IN1 de L298N a pin digital 2
int IN1 = 9;      // IN2 de L298N a pin digital 3
int ENA = 10;      // ENA de L298N a pin digital 5


void setup() {
  miniservo1.attach(12);
  miniservo2.attach(11);

  pinMode(IN3, OUTPUT);   // pin 2 como salida
  pinMode(IN4, OUTPUT);   // pin 3 como salida  
  pinMode(ENB, OUTPUT);   // pin 5 como salida

  pinMode(IN1, OUTPUT);   // pin 2 como salida
  pinMode(IN2, OUTPUT);   // pin 3 como salida  
  pinMode(ENA, OUTPUT);   // pin 5 como salida

  // coloca el servo en su posición central
  miniservo1.write(90);
  miniservo2.write(20);
  tiempo1 = millis();
}

void loop() {

  distancia = ultrasonic.Ranging(CM); //Lectura sensor de ultrasonidos

  if (distancia <= 20) {
    if (flag) {
      tiempo1 = millis();
      flag = false;
    }

    if (millis() - tiempo1 >= 3000UL) {  //si la distancia es menor de 20...
      tiempo1 = millis(); //Actualiza el tiempo actual
      //tiempoSegundos = tiempo1/5000;
      //movemos el servo en una dirección
  digitalWrite(ENB, HIGH);  // habilita motor A (giro en un sentido)
  digitalWrite(IN3, LOW); // IN1 a 0
  digitalWrite(IN4, HIGH);  // IN2 a 1

  digitalWrite(ENA, HIGH);  // habilita motor A (giro en un sentido)
  digitalWrite(IN2, LOW); // IN1 a 0
  digitalWrite(IN1, HIGH);  // IN2 a 1

      delay(5000);
  digitalWrite(ENB, LOW); // ENA en 0 para deshabilitar motor
  digitalWrite(ENA, LOW); // ENA en 0 para deshabilitar motor
      delay(2000);

      miniservo1.write(0);
      delay(4000);
      miniservo2.write(0);
      delay(4000);
      miniservo2.write(100);
      delay(4000);
      
  digitalWrite(ENB, HIGH);  // habilita motor A (giro en sentido contrario)
  digitalWrite(IN3, HIGH);  // IN1 a 1
  digitalWrite(IN4, LOW); // IN2 a 0

  digitalWrite(ENA, HIGH);  // habilita motor A (giro en sentido contrario)
  digitalWrite(IN2, HIGH);  // IN1 a 1
  digitalWrite(IN1, LOW); // IN2 a 0
  
      delay(2000);

  digitalWrite(ENB, LOW); // ENA en 0 para deshabilitar motor
  digitalWrite(ENA, LOW); // ENA en 0 para deshabilitar motor
    }

  } else { //si no hay nada, siguen en la posición inicial
    miniservo1.write(60);
    miniservo2.write(110);

  digitalWrite(ENB, LOW); // ENA en 0 para deshabilitar motor
  digitalWrite(ENA, LOW); // ENA en 0 para deshabilitar motor

    flag = true;
  }
  delay(500);
}

desde ya muchas gracias. saludos!

2

Moderador:
Por favor, lee las Normas del foro y edita tu titulo usando uno que no contenga palabras inútiles como Ayuda.
Lee el punto 5. de las normas.

3

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.