#include <Servo.h>
#define BUZZ 2
#define T1 3
#define T2 4
#define E1 5
#define E2 6
#define servoPin 9
#define LED 13
Servo servo;
float wall1, wall2;
int school = 0;
void setup()
{
Serial.begin(9600);
pinMode(BUZZ, OUTPUT);
pinMode(T1, OUTPUT);
pinMode(T2, OUTPUT);
pinMode(E1, INPUT);
pinMode(E2, INPUT);
pinMode(LED, OUTPUT);
servo.attach(servoPin);
get_dis(T1, E1);get_dis(T2, E2);get_dis(T1, E1);get_dis(T2, E2);
get_dis(T1, E1);get_dis(T2, E2);get_dis(T1, E1);get_dis(T2, E2);
Serial.println(wall1 = get_dis(T1, E1));
Serial.println(wall2 = get_dis(T2, E2));
}
void loop()
{
bool B = false;
long Time = 0;
while (1)
{
if (check_super(T1, E1, wall1) == 1)
{ B = true; Time = millis(); break; }
else if (check_super(T2, E2, wall2) == 1)
{ B = false; Time = millis(); break; }
if (check_super(T1, E1, wall1) == 2)
{ return; }
else if (check_super(T2, E2, wall2) == 2)
{ return; }
}
delay(50);
if (B)
while (1)
{
if (check_super(T2, E2, wall2) == 1)
{
Time = millis() - Time;
break;
}
else if (millis() - Time > 1000)
break;
if (check_super(T1, E1, wall1) == 2)
{ return; }
}
else
while (1)
{
if (check_super(T1, E1, wall1) == 1)
{
Time = millis() - Time;
break;
}
else if (millis() - Time > 1000)
break;
else if (check_super(T2, E2, wall2) == 2)
{ return; }
}
if (Time < 300)
{
BUZZER(2000);
digitalWrite(LED, LOW);
}
delay(1000);
}
int check_super(int tri, int echo, float wall)
{
delay(10);
float F = get_dis(tri, echo);
Serial.println(F);
int B = ((wall * (float)4) / (float)5 > F) ? 1 : 0;
B = (F > (float)1000) ? 2 : B;
delay(10);
return B;
}
float get_dis(int tri, int echo)
{
digitalWrite(tri, HIGH);
delayMicroseconds(15);
digitalWrite(tri, LOW);
delayMicroseconds(10);
float dis = 0.017 * pulseIn(echo, HIGH);
return dis;
}
void BUZZER(int T)
{
long Time = T + millis();
int delayed_T = 0, LED_state = HIGH;
while(1)
{
digitalWrite(BUZZ, HIGH);
delay(1);
digitalWrite(BUZZ, LOW);
delay(1);
if (millis() > Time)
break;
delayed_T += 2;
if (delayed_T > 100)
{
digitalWrite(LED, LED_state);
LED_state = (LED_state == HIGH) ? LOW : HIGH;
delayed_T = 0;
}
}
}
Comments
Please log in or sign up to comment.