52 lines
1.0 KiB
C++
52 lines
1.0 KiB
C++
#include <Servo.h>
|
|
|
|
Servo servo;
|
|
int trigger_pin = 11;
|
|
int echo_pin = 12;
|
|
int servo_pin = 7;
|
|
int r_led_pin = 4;
|
|
int g_led_pin = 5;
|
|
double distance = 0.0;
|
|
int current_led_status = 0;
|
|
|
|
void setup() {
|
|
// put your setup code here, to run once:
|
|
Serial.begin(9600);
|
|
pinMode(trigger_pin, OUTPUT);
|
|
pinMode(echo_pin, INPUT);
|
|
pinMode(r_led_pin, OUTPUT);
|
|
pinMode(g_led_pin, OUTPUT);
|
|
servo.attach(7);
|
|
digitalWrite(r_led_pin, HIGH);
|
|
digitalWrite(g_led_pin, HIGH);
|
|
}
|
|
|
|
void loop() {
|
|
// put your main code here, to run repeatedly:
|
|
digitalWrite(trigger_pin, HIGH);
|
|
delayMicroseconds(10);
|
|
digitalWrite(trigger_pin, LOW);
|
|
|
|
distance = pulseIn(echo_pin, HIGH) / 58.00;
|
|
|
|
Serial.print("Distance: ");
|
|
Serial.print(distance);
|
|
Serial.print("\n");
|
|
|
|
if (distance < 10) {
|
|
servo.write(90);
|
|
for (int i = 0; i < 5; i++) {
|
|
digitalWrite(r_led_pin, LOW);
|
|
delay(50);
|
|
digitalWrite(r_led_pin, HIGH);
|
|
delay(50);
|
|
}
|
|
}
|
|
else {
|
|
servo.write(0);
|
|
digitalWrite(g_led_pin, LOW);
|
|
delay(500);
|
|
digitalWrite(g_led_pin, HIGH);
|
|
}
|
|
}
|