QUADRUINO ROBOT QUADRUPED WALKING

1#include <Servo.h>
2
3// Definir pines del sensor ultrasónico
4const int trigPin = 5;
5const int echoPin = 6;
6
7// Definir pines de los servomotores
8const int servoPin1 = 9;
9const int servoPin2 = 10;
10
11// Definir distancia de detección
12const int distanceThreshold = 20; // Distancia de detección en centímetros
13
14// Crear objetos para los servomotores
15Servo servo1;
16Servo servo2;
17
18void setup() {
19  // Inicializar los pines del sensor ultrasónico
20  pinMode(trigPin, OUTPUT);
21  pinMode(echoPin, INPUT);
22
23  // Inicializar los servomotores
24  servo1.attach(servoPin1);
25  servo2.attach(servoPin2);
26
27  // Colocar los servos en su posición inicial
28  servo1.write(90); // Posición inicial de servo1 (90 grados)
29  servo2.write(90); // Posición inicial de servo2 (90 grados)
30}
31
32void loop() {
33  // Realizar la medición de distancia
34  long distance = getUltrasonicDistance();
35
36  // Verificar si hay un obstáculo a menos de 20 cm
37  if (distance < distanceThreshold) {
38    // Girar servo1 a la izquierda (ejemplo: 45 grados)
39    servo1.write(45);
40    delay(1000); // Puedes ajustar el tiempo de giro hacia la izquierda
41
42    // Detener servo1
43    servo1.write(90);
44
45    // Girar servo2 a la derecha (ejemplo: 135 grados)
46    servo2.write(135);
47    delay(1000); // Puedes ajustar el tiempo de giro hacia la derecha
48
49    // Detener servo2
50    servo2.write(90);
51  } else {
52    // Ambos servos avanzan hacia adelante (ejemplo: 0 grados)
53    servo1.write(0);
54    servo2.write(0);
55  }
56}
57
58long getUltrasonicDistance() {
59  // Generar un pulso de 10 microsegundos en el pin Trig
60  digitalWrite(trigPin, LOW);
61  delayMicroseconds(2);
62  digitalWrite(trigPin, HIGH);
63  delayMicroseconds(10);
64  digitalWrite(trigPin, LOW);
65
66  // Medir la duración del pulso en el pin Echo
67  long duration = pulseIn(echoPin, HIGH);
68
69  // Calcular la distancia en centímetros
70  long distance = duration * 0.034 / 2;
71
72  return distance;
73}