Part2
void sensorS()
{
long duration1, distance1;
digitalWrite(trigPin1, LOW);
delayMicroseconds(trigDelay);
digitalWrite(trigPin1, HIGH);
delayMicroseconds(trigDelay);
digitalWrite(trigPin1, LOW);
pulseTime1 = pulseIn(echoPin1, HIGH);
delay(echoDelay);
pD1 = (pulseTime1 * 0.0343) / (2.);
sensedHeight1 = oriHeight1 - pD1;
long duration2, distance2;
digitalWrite(trigPin2, LOW);
delayMicroseconds(trigDelay);
digitalWrite(trigPin2, HIGH);
delayMicroseconds(trigDelay);
digitalWrite(trigPin2, LOW);
pulseTime2 = pulseIn(echoPin2, HIGH);
delay(echoDelay);
pD2 = (pulseTime2 * 0.0343) / (2.);
sensedHeight2 = oriHeight2 - pD2;
long duration3, distance3;
digitalWrite(trigPin3, LOW);
delayMicroseconds(trigDelay);
digitalWrite(trigPin3, HIGH);
delayMicroseconds(trigDelay);
digitalWrite(trigPin3, LOW);
pulseTime3 = pulseIn(echoPin3, HIGH);
delay(echoDelay);
pD3 = (pulseTime3 * 0.0343) / (2.);
sensedHeight3 = oriHeight3 - pD3;
if(sensedHeight1 >= humanHeight) //people is standing under the sensor, or is passing by
{
sensed1 = true; //will turn into false if no being used or after used or the person is passby sensor1
}
if(sensedHeight2 >= humanHeight)
{
sensed2 = true;
}
if(sensedHeight3 >= humanHeight)
{
sensed3 = true;
}
if(sensed1 == true && sensedHeight1 < humanHeight) //if the person is sensed, then go away
{
sensorValue1++;
timer1.start(timerValue); //to clear sensorValue if not being used
delay(sensedDelay);
sensed1 = false;
}
if(sensed2 == true && sensedHeight2 < humanHeight)
{
sensorValue2++;
delay(sensedDelay);
timer2.start(timerValue);
sensed2 = false;
}
if(sensed3 == true && sensedHeight3 < humanHeight)
{
sensorValue3++;
timer3.start(timerValue);
delay(sensedDelay);
sensed3 = false;
}
if (timer1.justFinished() && sensed1 == false && sensorValue2 == 0) //to clear sensorValue
{
sensorValue1 = 0;
}
if (timer2.justFinished() && sensed2 == false)
//the most important sensorValue to indicate whether the person is actually going in, or turn back in the middle, or going out
{
sensorValue2 = 0;
}
if (timer3.justFinished() && sensed3 == false && sensorValue2 == 0)
{
sensorValue3 = 0;
}
//THE GOING IN PROCESS//
if (sensorValue1 >= 1 && sensed1 == true)
//if the person1 is going in, sensorValue is = [1,0,0], boolean = [true, false, false]
{
if (sensorValue2 >= 1 && sensed2 == true && (sensed1 == true || sensed1 == false))
//then the person1 keep going, maybe there is another person2 going in too, or maybe not, sensorValue is = [1,1,0] or [2,1,0] , boolean = [false, true, false] or [true, true, false]
{
if (sensorValue3 >= 1 && (sensed2 != true && sensed3 == true))
//if the person1 finally passed sensor1, sensor2, then reached sensor3, sensorValue is = [1,1,1] or [2,1,1] or [2,2,1], boolean = [false, false, true] or [true, false, true] or [true, true, true]
{
peopleCounted++;
sensorValue1--;
sensorValue2--;
}
else if (sensorValue3 >= 1 && (sensed2 == true && sensed3 == true))
//if the person1 at sensor2 is being block by another person at sensor3, so sensorValue is also = [1,1,1], boolean = [false, true, true]
{
if (sensed2 == false && sensorValue3 >= 2 && sensed3 == true )
//if another person let person1 go in, then sensorValue is = [1,1,2], boolean = [false, false, true]
{
if (sensed1 != true)
//to ensure the person is actual going in, not turn back in the middle, if turn back sensorValue = [2,1,1] or [2,1,0], boolean = [true, false, true] or [true, false, false]
{
peopleCounted++;
sensorValue1--;
sensorValue2--;
}
}
}
}
}
//THE GOING OUT PROCESS// it's actual the reverse mode of GOING IN PROCESS
if (sensorValue3 >= 1 && sensed3 == true)
//if personA is going out from sensor3, sensorValue is = [0,0,1], boolean = [false, false, true]
{
if (sensorValue2 >= 1 && sensed2 == true && (sensed3 == true || sensed3 == false))
//and personA reached sensor2, maybe there is another person following, or maybe not, sensorValue is = [0,1,1] or [0,1,2], boolean = [false, true, false] or [false, true, true]
{
if (sensorValue1 >= 1 && (sensed2 != true && sensed1 == true))
//finally the personA going out sucessfully and reached sensor1, sensorValue is = [1,1,1] or [1,1,2] or [1,2,2] , boolean = [true, false, false] or [true, false, true] or [true, true, true]
{
peopleCounted--;
sensorValue3--;
sensorValue2--;
}
else if (sensorValue1 >= 1 && (sensed1 == true && sensed2 == true))
//maybe the personA at sensor2 is being block by another personB at sensor1 when going out, so sensorValue is also = [1,1,1] , boolean = [true, true, false]
{
if (sensed2 == false && sensorValue1 >= 2 && sensed1 == true)
//then if personB leave and let personA going out, then sensorValue is = [2,1,1], boolean = [true, false, false]
{
if (sensed3 != true)
//to ensure the personA is actual going out, not turn back in the middle, if turn back sensorValue = [1,1,2] or [0,1,2] boolean = [true, false, true] or [false, false, true]
{
peopleCounted--;
sensorValue3--;
sensorValue2--;
}
}
}
}
}
}
//________________________________________________________________
void setup()
{
Serial.begin(BAUD_RATE);
for (int i = 10; i > 0; i--)
{
Serial.print(i);
Serial.print(" ");
Serial.println(" ");
delay(500);
}
pinMode (trigPin1, OUTPUT);
pinMode (echoPin1, INPUT);
pinMode (trigPin2, OUTPUT);
pinMode (echoPin2, INPUT);
pinMode (trigPin3, OUTPUT);
pinMode (echoPin3, INPUT);
oriDelay.start(10000); //use 10 sec to measure the oriHeight
printerCounter.start(2000); //repeat every 2 second to check status on Serial Monitor
}
//________________________________________________________________
void valuePrinter()
{
if(printerCounter.justFinished())
{
printerCounter.repeat();
Serial.print("OH1: ");Serial.println(oriHeight1);
Serial.print("OH2: ");Serial.println(oriHeight2);
Serial.print("OH3: ");Serial.println(oriHeight3);
Serial.print("PD1: "); Serial.println(pD1);
Serial.print("PD2: "); Serial.println(pD2);
Serial.print("PD3: "); Serial.println(pD3);
Serial.print("SH1: "); Serial.println(sensedHeight1);
Serial.print("SH2: "); Serial.println(sensedHeight2);
Serial.print("SH3: "); Serial.println(sensedHeight3);
Serial.print("SD1: "); Serial.println(sensed1);
Serial.print("SD2: "); Serial.println(sensed2);
Serial.print("SD3: "); Serial.println(sensed3);
Serial.print("SV1: "); Serial.println(sensorValue1);
Serial.print("SV2: "); Serial.println(sensorValue2);
Serial.print("SV3: "); Serial.println(sensorValue3);
Serial.print("PC: "); Serial.println(peopleCounted);
}
}
//________________________________________________________________
void loop()
{
if (startUpDone != true)
{
startUp();
}
else
{
sensorS();
}
valuePrinter(); //use to check status on Serial Monitor
}