Practical case: Arduino UNO Servo Calibration Tester

/*
 * Project: Servo Calibration Tester
 * Target: Arduino UNO R3 (ATmega328P)
 * Description: A multi-mode servo testing tool. Uses a potentiometer for manual
 * control and a pushbutton to cycle between Manual, Center, and Sweep modes.
 */

#include <Servo.h>

// Pin Definitions
const int SERVO_PIN = 9;
const int POT_PIN = A0;
const int BUTTON_PIN = 2;

// Servo Object
Servo testServo;

// State Machine Variables
enum TesterMode {
  MODE_MANUAL = 0,
  MODE_CENTER = 1,
  MODE_SWEEP  = 2
};

TesterMode currentMode = MODE_MANUAL;

// Button Debouncing Variables
int buttonState = HIGH;             // Current reading from the input pin
int lastButtonState = HIGH;         // Previous reading from the input pin
unsigned long lastDebounceTime = 0; // The last time the output pin was toggled
const unsigned long debounceDelay = 50;   // Debounce time in milliseconds

// Servo Sweep Variables
int sweepAngle = 0;
int sweepDirection = 1;
unsigned long lastSweepUpdate = 0;
const unsigned long sweepInterval = 15; // Milliseconds between sweep steps

// Telemetry Variables
unsigned long lastTelemetryTime = 0;
const unsigned long telemetryInterval = 500; // Update Serial every 500ms

void setup() {
  // Initialize Serial Monitor
  Serial.begin(115200);
  while (!Serial) {
    ; // Wait for serial port to connect
  }

  Serial.println("Initializing Servo Calibration Tester...");

  // Configure Pins
  pinMode(POT_PIN, INPUT);
  pinMode(BUTTON_PIN, INPUT_PULLUP); // Use internal pull-up resistor

  // Attach Servo
  // Using standard min/max pulse widths (1000us to 2000us)
  // Some SG90 servos respond better to 500us to 2400us, adjust if necessary.
  testServo.attach(SERVO_PIN, 1000, 2000);

  // Set initial position
  testServo.write(90);
  Serial.println("System Ready. Mode: MANUAL");
}

void loop() {
  handleButton();
  updateServo();
  sendTelemetry();
}

void handleButton() {
  int reading = digitalRead(BUTTON_PIN);

  // Check if the button state has changed
  if (reading != lastButtonState) {
    lastDebounceTime = millis();
  }

  // If the state has been stable longer than the debounce delay
  if ((millis() - lastDebounceTime) > debounceDelay) {
    // If the button state has physically changed
    if (reading != buttonState) {
      buttonState = reading;

      // Only toggle mode if the new button state is LOW (pressed)
      if (buttonState == LOW) {
        cycleMode();
      }
    }
  }
  lastButtonState = reading;
}

void cycleMode() {
  if (currentMode == MODE_MANUAL) {
    currentMode = MODE_CENTER;
    Serial.println(">>> Mode Switched: CENTER (90 deg) <<<");
  } else if (currentMode == MODE_CENTER) {
    currentMode = MODE_SWEEP;
    Serial.println(">>> Mode Switched: AUTO-SWEEP <<<");
  } else {
    currentMode = MODE_MANUAL;
    Serial.println(">>> Mode Switched: MANUAL <<<");
  }
}

void updateServo() {
  switch (currentMode) {
    case MODE_MANUAL:
      {
        int potValue = analogRead(POT_PIN);
        // Map 10-bit ADC (0-1023) to Servo Angle (0-180)
        int targetAngle = map(potValue, 0, 1023, 0, 180);
        testServo.write(targetAngle);
      }
      break;

    case MODE_CENTER:
      {
        testServo.write(90);
      }
      break;

    case MODE_SWEEP:
      {
        if (millis() - lastSweepUpdate > sweepInterval) {
          lastSweepUpdate = millis();
          sweepAngle += sweepDirection;

          if (sweepAngle >= 180) {
            sweepAngle = 180;
            sweepDirection = -1;
          } else if (sweepAngle <= 0) {
            sweepAngle = 0;
            sweepDirection = 1;
          }
          testServo.write(sweepAngle);
        }
      }
      break;
  }
}

void sendTelemetry() {
  if (millis() - lastTelemetryTime > telemetryInterval) {
    lastTelemetryTime = millis();

    int currentAngle = testServo.read();
    int currentMicroseconds = testServo.readMicroseconds();

    Serial.print("Mode: ");
    switch (currentMode) {
      case MODE_MANUAL: Serial.print("MANUAL\t"); break;
      case MODE_CENTER: Serial.print("CENTER\t"); break;
      case MODE_SWEEP:  Serial.print("SWEEP \t"); break;
    }

    Serial.print(" | Angle: ");
    Serial.print(currentAngle);
    Serial.print(" deg | Pulse: ");
    Serial.print(currentMicroseconds);
    Serial.println(" us");
  }
}