Auto/Manual obstacle detector

1#include <Servo.h>
2
3//Servo  
4Servo scanner;
5const int servoPin = 9;
6
7//Joystick  
8const int joyX = A0;
9const int joyY = A1;
10const int joySW = 2;
11
12//Ultrasonic  
13const int trigPin = 6;
14const int echoPin = 7;
15
16//Buzzer  
17const int buzzerPin = 5;
18const int alertDist = 25; //cm
19
20//System State  
21int mode = 0;    //0 = manual, 1 = auto scan
22int angle = 90;
23int scanDir = 1;
24
25unsigned long lastButtonTime = 0;
26
27void setup() {
28  scanner.attach(servoPin);
29
30  pinMode(trigPin, OUTPUT);
31  pinMode(echoPin, INPUT);
32
33  pinMode(joySW, INPUT_PULLUP);
34  pinMode(buzzerPin, OUTPUT);
35
36  Serial.begin(9600); 
37}
38
39//Ultrasonic Distance  
40long readDistance() {
41  digitalWrite(trigPin, LOW);
42  delayMicroseconds(2);
43  digitalWrite(trigPin, HIGH);
44  delayMicroseconds(10);
45  digitalWrite(trigPin, LOW);
46
47  long duration = pulseIn(echoPin, HIGH, 25000); //timeout
48  if (duration == 0) return -1;
49
50  return duration / 58; //cm
51}
52
53//Alarm Logic  
54void alarmCheck(long distance) {
55  if (distance > 0 && distance < alertDist) {
56  int interval = map(distance, 1, alertDist, 50, 400);
57  digitalWrite(buzzerPin, HIGH);
58  delay(interval);
59  digitalWrite(buzzerPin, LOW);
60  delay(interval);
61  } else {
62  digitalWrite(buzzerPin, LOW);
63  }
64}
65
66void loop() {
67  int xVal = analogRead(joyX);
68  int yVal = analogRead(joyY);
69
70  //Button: Mode Switch
71  if (digitalRead(joySW) == LOW && millis() - lastButtonTime > 300) {
72  mode = !mode;
73  lastButtonTime = millis();
74  }
75
76  //Manual Mode  
77  if (mode == 0) {
78  angle = map(xVal, 0, 1023, 30, 150);
79  }
80
81  //Auto Scan Mode  
82  if (mode == 1) {
83  int speedDelay = map(yVal, 0, 1023, 5, 30);
84  angle += scanDir;
85
86  if (angle >= 150 || angle <= 30) {
87  scanDir *= -1;
88  }
89  delay(speedDelay);
90  }
91
92  scanner.write(angle);
93
94  long distance = readDistance();
95
96  //Alarm  
97  alarmCheck(distance);
98
99  //Debug Output (optional)  
100  Serial.print("Mode: ");
101  Serial.print(mode == 0 ? "Manual" : "Auto");
102  Serial.print(" | Angle: ");
103  Serial.print(angle);
104  Serial.print(" | Distance: ");
105  Serial.println(distance);
106
107  delay(10);
108}