#include <Ultrasonic.h>
#include <Servo.h>
// Pins used by your servos. Adjust according to your connections.
#define pin_servo1 3
#define pin_servo2 4
// Pins used by your ultrasonic sensors. Adjust according to your connections.
#define pin_trigger1 5
#define pin_trigger2 6
#define pin_echo1 7
#define pin_echo2 8
float v1_dist_cm, v1_dist_in, v2_dist_cm, v2_dist_in;
Servo srv1;
Servo srv2;
Ultrasonic ultra1(pin_trigger1, pin_echo1);
Ultrasonic ultra2(pin_trigger2, pin_echo2);
void setup()
{
srv1.attach(pin_servo1);
srv2.attach(pin_servo2);
srv1.write(0);
srv2.write(0);
}
void loop()
{
// Read ultrasonic values
long microsec1 = ultra1.timing();
long microsec2 = ultra2.timing();
// Distance in inches and in centimeters
v1_dist_cm = ultra1.convert(microsec1, Ultrasonic::CM);
v1_dist_in = ultra1.convert(microsec1, Ultrasonic::IN);
v2_dist_cm = ultra2.convert(microsec2, Ultrasonic::CM);
v2_dist_in = ultra2.convert(microsec2, Ultrasonic::IN);
// According to your readings, you move the servos:
if (v1_dist_cm <= 7 && v2_dist_cm > 7) {
srv1.write(20);
}
else
{
srv1.write(0);
}
if (v1_dist_cm > 7 && v2_dist_cm <= 7) {
srv1.write(20);
srv2.write(40);
}
else
{
srv1.write(0);
srv2.write(0);
}
if (v1_dist_cm <= 7 && v2_dist_cm <= 7) {
// Put your “program 2” here.
}
}