/*
  Servo Calibration
*/

#include <Servo.h>

const int SERVO_PIN = 9;

int MIN_US = 500;
int MAX_US = 2500;

Servo myServo;
int currentPulse = 1500;

void setup() {
  Serial.begin(9600);
  myServo.attach(SERVO_PIN, MIN_US, MAX_US);
  myServo.writeMicroseconds(currentPulse);

  Serial.println("Servo Calibration");
  Serial.println("Send a pulse width in microseconds (e.g. 1500) + Enter.");
  Serial.println("'s' = sweep, 'c' = center, 'p' = print current MIN/MAX");
}

void loop() {
  if (Serial.available()) {
    if (Serial.peek() == 's' || Serial.peek() == 'c' || Serial.peek() == 'p') {
      char cmd = Serial.read();
      handleCommand(cmd);
    } else {
      int val = Serial.parseInt();
      if (val > 0) {
        currentPulse = constrain(val, 400, 2600); // safety clamp beyond nominal range
        myServo.writeMicroseconds(currentPulse);
        Serial.print("Pulse: ");
        Serial.println(currentPulse);
      }
    }
    while (Serial.available()) Serial.read(); // clear leftover newline/chars
  }
}

void handleCommand(char cmd) {
  switch (cmd) {
    case 's':
      Serial.println("Sweeping...");
      for (int us = MIN_US; us <= MAX_US; us += 10) {
        myServo.writeMicroseconds(us);
        delay(20);
      }
      for (int us = MAX_US; us >= MIN_US; us -= 10) {
        myServo.writeMicroseconds(us);
        delay(20);
      }
      currentPulse = (MIN_US + MAX_US) / 2;
      myServo.writeMicroseconds(currentPulse);
      break;

    case 'c':
      currentPulse = (MIN_US + MAX_US) / 2;
      myServo.writeMicroseconds(currentPulse);
      Serial.print("Center: ");
      Serial.println(currentPulse);
      break;

    case 'p':
      Serial.print("MIN_US: "); Serial.println(MIN_US);
      Serial.print("MAX_US: "); Serial.println(MAX_US);
      break;
  }
}
