// Sensor
int S1Echo = 2;
int S1Trig = 3;
int S2Echo = 8;
int S2Trig = 9;
// Relay
int R1 = 50;
int R2 = 51;
int R3 = 52;
int R4 = 53;
//Variable
//Distance
int iS1Distance;
int iS1Duration;
int iS2Distance;
int iS2Duration;
int D1 = 500;
int D2 = 1000;
//State
int iS1State;
String sS1State;
int iS2State;
String sS2State;
//Temperature
int iTemperature = 30;
//Time
int iStartTime;
int iFinishTime;
int iTactTime;
void setup() {
// put your setup code here, to run once:
Serial.begin(9600);
pinMode(S1Echo, INPUT);
pinMode(S1Trig, OUTPUT);
pinMode(S2Echo, INPUT);
pinMode(S2Trig, OUTPUT);
pinMode(R1, OUTPUT);
pinMode(R2, OUTPUT);
pinMode(R3, OUTPUT);
pinMode(R4, OUTPUT);
}
void loop() {
iStartTime = millis();
delay(30);
digitalWrite(S1Trig, HIGH);
delayMicroseconds(10);
digitalWrite(S1Trig, LOW);
iS1Duration = pulseIn(S1Echo, HIGH, 14000);
iS1Distance = (330 + (0.6 * iTemperature)) * iS1Duration * 0.0005;
delay(30);
digitalWrite(S2Trig, HIGH);
delayMicroseconds(10);
digitalWrite(S2Trig, LOW);
iS2Duration = pulseIn(S2Echo, HIGH, 14000);
iS2Distance = (330 + (0.6 * iTemperature)) * iS2Duration * 0.0005;
if (iS1Distance > 0 && iS1Distance <= D1) {
iS1State = 11;
sS1State = “ST”;
}
else if (iS1Distance > D1 && iS1Distance <= D2){
iS1State = 12;
sS1State = “DC”;
}
else {
iS1State = 10;
sS1State = “CV”;
}
if (iS2Distance > 0 && iS2Distance <= D1) {
iS2State = 21;
sS2State = “ST”;
}
else if (iS1Distance > D1 && iS1Distance <= D2){
iS2State = 22;
sS2State = “DC”;
}
else {
iS2State = 20;
sS2State = “CV”;
}
switch (iS1State) {
case 11:
digitalWrite(R1, HIGH);
digitalWrite(R2, LOW);
break;
case 12:
digitalWrite(R1, LOW);
digitalWrite(R2, HIGH);
break;
case 10:
digitalWrite(R1, LOW);
digitalWrite(R2, LOW);
}
switch (iS2State) {
case 21:
digitalWrite(R3, HIGH);
digitalWrite(R4, LOW);
break;
case 22:
digitalWrite(R3, LOW);
digitalWrite(R4, HIGH);
break;
case 20:
digitalWrite(R3, LOW);
digitalWrite(R4, LOW);
}
iFinishTime = millis();
iTactTime = iFinishTime – iStartTime;
Serial.print(“TactTime : “);
Serial.print(iTactTime);
Serial.print(” “);
Serial.print(“S1 : “);
Serial.print(iS1Distance);
Serial.print(” “);
Serial.print(“S2 : “);
Serial.print(iS2Distance);
Serial.print(” “);
Serial.print(“S1State : “);
Serial.print(iS1State);
Serial.print(” “);
Serial.print(“S2State : “);
Serial.println(iS2State);
}