Full system — manual, sweep and homing

#include <Servo.h>

// --- Pins ---
const int POT_PIN    = A0;
const int SERVO_PIN  = 9;
const int TRIG_PIN   = 12;
const int ECHO_PIN   = 11;

// Radar Distance Feedback LEDs
const int RED_LED    = 2;
const int BLUE_LED   = 3;
const int ORANGE_LED = 4;
const int GREEN_LED  = 5;

// Touch Interface Pins
const int TOUCH_PIN  = 10;
const int MODE_LED_1 = 6;
const int MODE_LED_2 = 7;
const int MODE_LED_3 = 8;

// --- Calibration Limits ---
const int SERVO_MIN  = 15;   
const int SERVO_MAX  = 165;  

// --- Global Variables ---
Servo myServo;
float currentDistance = 0.0;
long echoDuration = 0;
int currentServoAngle = 90; // Tracks the current position of the servo arm

// Touch & Mode State Variables
int activeMode = 1;           // Mode 1 = Manual, Mode 2 = Sweep, Mode 3 = Homing
int currentTouchState = LOW;  
int lastTouchState = LOW;     

// Auto-Sweep Direction Trackers
int sweepDirection = 1;       // 1 = sweeping up, -1 = sweeping down

void setup() {
  Serial.begin(9600);
  
  myServo.attach(SERVO_PIN);
  myServo.write(currentServoAngle);
  
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  
  pinMode(RED_LED, OUTPUT);
  pinMode(BLUE_LED, OUTPUT);
  pinMode(ORANGE_LED, OUTPUT);
  pinMode(GREEN_LED, OUTPUT);
  
  pinMode(TOUCH_PIN, INPUT);
  pinMode(MODE_LED_1, OUTPUT);
  pinMode(MODE_LED_2, OUTPUT);
  pinMode(MODE_LED_3, OUTPUT);
  
  updateModeIndicators();
}

void loop() {
  // Check if user changed the operating mode via the touch sensor
  checkTouchSensor();
  
  // Every mode requires distance tracking to drive the feedback LEDs
  readDistance();   
  updateRadarLEDs(); 

  // Branch execution into the chosen system behavior
  switch (activeMode) {
    case 1:
      runManualMode();
      break;
    case 2:
      runSweepMode();
      break;
    case 3:
      runHomingMode();
      break;
  }
  
  delay(30); // Stabilizes the servo movement and avoids spamming the execution loop
}

// --- CORE SYSTEM MODES ---

// Mode 1: Manual Potentiometer Turning
void runManualMode() {
  int potValue = analogRead(POT_PIN);
  currentServoAngle = map(potValue, 0, 1023, SERVO_MIN, SERVO_MAX);
  myServo.write(currentServoAngle);
}

// Mode 2: Constant Automated Sweep
void runSweepMode() {
  // Step the angle by 1 degree per execution slice
  currentServoAngle += sweepDirection;
  
  // Reverse when boundaries are intersected
  if (currentServoAngle >= SERVO_MAX) {
    currentServoAngle = SERVO_MAX;
    sweepDirection = -1;
  } 
  else if (currentServoAngle <= SERVO_MIN) {
    currentServoAngle = SERVO_MIN;
    sweepDirection = 1;
  }
  
  myServo.write(currentServoAngle);
}

// Mode 3: Dynamic Object Homing
void runHomingMode() {
  bool isTargetInSweetSpot = (currentDistance >= 20.0 && currentDistance <= 30.0 && echoDuration > 0);
  
  if (isTargetInSweetSpot) {
    // Lock the servo in position; an object is successfully tracked in the green zone!
    return; 
  }
  
  // If the target is lost or out of the sweetspot, execute standard search sweeping
  currentServoAngle += sweepDirection;
  
  if (currentServoAngle >= SERVO_MAX) {
    currentServoAngle = SERVO_MAX;
    sweepDirection = -1;
  } 
  else if (currentServoAngle <= SERVO_MIN) {
    currentServoAngle = SERVO_MIN;
    sweepDirection = 1;
  }
  
  myServo.write(currentServoAngle);
}

// --- CORE UTILITY FUNCTIONS ---

void checkTouchSensor() {
  currentTouchState = digitalRead(TOUCH_PIN);

  if (currentTouchState == HIGH && lastTouchState == LOW) {
    activeMode++;
    if (activeMode > 3) {
      activeMode = 1;
    }
    updateModeIndicators();
    
    Serial.print("--- Mode Shifted to: ");
    Serial.println(activeMode);
    delay(150); // Software debounce to eliminate contact bounce noise
  }
  lastTouchState = currentTouchState;
}

void updateModeIndicators() {
  digitalWrite(MODE_LED_1, LOW);
  digitalWrite(MODE_LED_2, LOW);
  digitalWrite(MODE_LED_3, LOW);

  if (activeMode == 1)      digitalWrite(MODE_LED_1, HIGH);
  else if (activeMode == 2) digitalWrite(MODE_LED_2, HIGH);
  else if (activeMode == 3) digitalWrite(MODE_LED_3, HIGH);
}

void readDistance() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  
  echoDuration = pulseIn(ECHO_PIN, HIGH, 25000); // 25ms timeout handles maximum room scale scanning cleanly
  currentDistance = echoDuration * 0.0343 / 2.0;
}

void updateRadarLEDs() {
  digitalWrite(RED_LED, LOW);
  digitalWrite(BLUE_LED, LOW);
  digitalWrite(ORANGE_LED, LOW);
  digitalWrite(GREEN_LED, LOW);

  if (echoDuration == 0 || currentDistance <= 0) {
    digitalWrite(RED_LED, HIGH);
  }
  else if (currentDistance > 0 && currentDistance < 2) {
    digitalWrite(BLUE_LED, HIGH);
  }
  else if (currentDistance >= 20 && currentDistance <= 30) {
    digitalWrite(GREEN_LED, HIGH);
  }
  else if ((currentDistance >= 2 && currentDistance < 20) || (currentDistance > 30 && currentDistance <= 50)) {
    digitalWrite(ORANGE_LED, HIGH);
  }
}