Arduino Obstacle-Avoiding Robot with Servo Motors and Ultrasonic Sensor
Arduino Obstacle-Avoiding Robot Code
#include <Servo.h>
Servo myservo; // crea el objeto servo
Servo rDerecha;
Servo rIzquierda;
#define irDerecho 3
#define irIzquierdo 2
#define ruedaD 9
#define ruedaI 8
#define echoPin 5
#define triggerPin 4
void setup() {
// put your setup code here, to run once:
pinMode(echoPin, INPUT);
pinMode(triggerPin, OUTPUT);
pinMode(irDerecho, INPUT);
pinMode(irIzquierdo, INPUT);
rDerecha.attach(ruedaD);
rIzquierda.attach(ruedaI);
myservo.attach(13); // vincula el servo al pin digital 13
}
void loop() {
// put your main code here, to run repeatedly
int valor;
valor = ping(triggerPin, echoPin);
if (valor < 10) {
parar(rDerecha, rIzquierda);
} else {
if (digitalRead(irDerecho) == LOW && digitalRead(irIzquierdo) == LOW) {
moverrecto(rDerecha, rIzquierda);
} else {
if (digitalRead(irDerecho) == HIGH) {
moverderecha(rDerecha, rIzquierda);
} else {
moverizquierda(rDerecha, rIzquierda);
}
}
}
}
void parar(Servo DER, Servo IZ) {
DER.write(90);
IZ.write(90);
}
void moverrecto(Servo DER, Servo IZ) {
DER.write(180);
IZ.write(0);
}
void moverderecha(Servo DER, Servo IZ) {
DER.write(90);
IZ.write(0);
}
void moverizquierda(Servo DER, Servo IZ) {
DER.write(180);
IZ.write(90);
}
void rotarizquierda(Servo DER, Servo IZ) {
DER.write(180);
IZ.write(180);
}
void rotarderecha(Servo DER, Servo IZ) {
DER.write(0);
IZ.write(0);
}
void retroceder(Servo DER, Servo IZ) {
DER.write(0);
IZ.write(180);
}
void pito(int buzz) {
tone(buzz, 550, 150);
delay(200);
tone(buzz, 550, 150);
delay(100);
}
int ping(int TriggerPin, int EchoPin) {
long duration, distanceCm;
digitalWrite(TriggerPin, LOW); // para generar un pulso limpio ponemos a LOW 4us
delayMicroseconds(4);
digitalWrite(TriggerPin, HIGH); // generamos Trigger (disparo) de 10us
delayMicroseconds(10);
digitalWrite(TriggerPin, LOW);
duration = pulseIn(EchoPin, HIGH); // medimos el tiempo entre pulsos, en microsegundos
distanceCm = duration * 10 / 292 / 2; // convertimos a distancia, en cm
return distanceCm;
}