Servo y Ultrasonido

[Robert]

Código [Seleccionar] Expandir

#include <Servo.h>
Servo myservo;
#define TRIG_PIN 3
#define ECHO_PIN 5
int led = 13; // Definimos la variable para el pin del LED
int BZ = 10; //
void setup() {
pinMode(led, OUTPUT); // Configuramos el pin del LED como salida
pinMode(BZ, OUTPUT);
myservo.attach(4);
Serial.begin(9600);
}
void loop() {
long duration, distancia;
// Generar un pulso de 10 microsegundos en el pin Trig
pinMode(TRIG_PIN, OUTPUT);
digitalWrite(TRIG_PIN, LOW);
delayMicroseconds(2);
digitalWrite(TRIG_PIN, HIGH);
delayMicroseconds(10);
digitalWrite(TRIG_PIN, LOW);

// Medir la duración del pulso en el pin Echo
pinMode(ECHO_PIN, INPUT);
duration = pulseIn(ECHO_PIN, HIGH);
// Calcular la distancia en centímetros
distancia = duration * 0.034 / 2;
//Serial.print(distancia);
// Serial.print("cm");
 // Serial.println();
  delay(500);

if (distancia <= 40 )
{
  myservo.write(170);
  digitalWrite(led, HIGH); // Encendemos el LED
  digitalWrite(BZ, HIGH); //
   delay(100);
   digitalWrite(BZ, LOW); //
   delay(100);
   digitalWrite(BZ, HIGH); //
   delay(100);
   digitalWrite(BZ, LOW); //
delay(100);
digitalWrite(BZ, HIGH); //
delay(100);

  }
if (distancia <= 70 && distancia >=41  )
{digitalWrite(led, HIGH); // Encendemos el LED
   myservo.write(100);
   digitalWrite(BZ, HIGH); //
   delay(50);
   digitalWrite(BZ, LOW); //
   delay(50);
   digitalWrite(BZ, HIGH); //
   delay(50);
   digitalWrite(BZ, LOW); //
   
  }

  if (distancia <= 120 )
{digitalWrite(led, HIGH); // Encendemos el LED
   delay(50);
  digitalWrite(BZ, HIGH); //
   delay(50);
   digitalWrite(BZ, LOW); //
   myservo.write(20);
    }
else{
 digitalWrite(led, LOW); // Encendemos el LED
 digitalWrite(BZ, LOW); //
myservo.write(0);
//delay(50);
}
}