int trig1 = 3;
int echo1 = 5;
int trig2 = 6;
int echo2 = 9;
int trig3 = 10;
int echo3 = 11;
int mA1 = 4;
int mA2 = 7;
int mB1 = 12;
int mB2 = 13;
float pingtime1;
float distance1;
float pingtime2;
float distance2;
float pingtime3;
float distance3;
float speedofsound = 13397.2441;
void setup() {
Serial.begin(9600);
Serial.println("Measuring the distance1");
Serial.println("Measuring the distance2");
Serial.println("Measuring the distance3");
pinMode(trig1, OUTPUT);
pinMode(echo1, INPUT);
pinMode(trig2, OUTPUT);
pinMode(echo2, INPUT);
pinMode(trig3, OUTPUT);
pinMode(echo3, INPUT);
pinMode(mA1, OUTPUT);
pinMode(mA2, OUTPUT);
pinMode(mB1, OUTPUT);
pinMode(mB2, OUTPUT);
}
void loop() {
digitalWrite(trig1, LOW);
delayMicroseconds(3000);
digitalWrite(trig1, HIGH);
delayMicroseconds(10);
digitalWrite(trig1, LOW);
pingtime1 = pulseIn(echo1, HIGH);
pingtime1 = pingtime1 / 1000000.;
distance1 = pingtime1 * speedofsound;
distance1 = distance1 / 2; //because the pulse have travelled the same distance twice
digitalWrite(trig2, LOW);
delayMicroseconds(3000);
digitalWrite(trig2, HIGH);
delayMicroseconds(10);
digitalWrite(trig2, LOW);
pingtime2 = pulseIn(echo2, HIGH);
pingtime2 = pingtime2 / 1000000.;
distance2 = pingtime2 * speedofsound;
distance2 = distance2 / 2; //because the pulse have travelled the same distance twice
digitalWrite(trig3, LOW);
delayMicroseconds(3000);
digitalWrite(trig3, HIGH);
delayMicroseconds(10);
digitalWrite(trig3, LOW);
pingtime3 = pulseIn(echo3, HIGH);
pingtime3 = pingtime3 / 1000000.;
distance3 = pingtime3 * speedofsound;
distance3 = distance3 / 2;
if (distance1 > 5 && distance2 > 5 && distance3 > 5)
{
digitalWrite(mA2, HIGH);
digitalWrite(mA1, LOW);
digitalWrite(mB2, HIGH);
digitalWrite(mB1, LOW);
}
else if (distance1 > 5 && distance2 > 5 && distance3 < 5)
{
digitalWrite(mA1, LOW);
digitalWrite(mA2, HIGH);
digitalWrite(mB1, HIGH);
digitalWrite(mB2, LOW);
delay(5000);
}
else if (distance1 > 5 && distance2 < 5 && distance3 > 5)
{
digitalWrite(mA1, HIGH);
digitalWrite(mA2, LOW);
digitalWrite(mB1, LOW);
digitalWrite(mB2, HIGH);
delay(5000);
}
else if (distance1 < 5 && distance2 > 5 && distance3 > 5)
{
digitalWrite(mA1, HIGH);
digitalWrite(mA2, LOW);
digitalWrite(mB1, LOW);
digitalWrite(mB2, HIGH);
delay(5000);
}
else if (distance1 > 5 && distance2 < 5 && distance3 < 5)
{
digitalWrite(mA1, HIGH);
digitalWrite(mA2, LOW);
digitalWrite(mB1, HIGH);
digitalWrite(mB2, LOW);
delay(5000);
}
else if (distance1 < 5 && distance2 > 5 && distance3 < 5)
{
digitalWrite(mA1, LOW);
digitalWrite(mA2, HIGH);
digitalWrite(mB1, HIGH);
digitalWrite(mB2, LOW);
delay(5000);
}
else if (distance1 < 5 && distance2 < 5 && distance3 > 5)
{
digitalWrite(mA1, HIGH);
digitalWrite(mA2, LOW);
digitalWrite(mB1, LOW);
digitalWrite(mB2, HIGH);
delay(5000);
}
else if (distance1 < 5 && distance2 < 5 && distance3 < 5)
{
digitalWrite(mA2, HIGH);
digitalWrite(mA1, LOW);
digitalWrite(mB2, HIGH);
digitalWrite(mB1, LOW);
delay(5000);
}
}