/*****************************************************************************
* Servo Motor Control
* Code to control a servo motor's position - manual.
*
* Author: Kelen C. Teixeira Vivaldini
******************************************************************************/

#define SERVO_PIN 6 // Pin connected to the servo
#define PWM_FREQ 50 // PWM frequency in Hertz (50Hz for servos)

void setup() {
  pinMode(SERVO_PIN, OUTPUT); // Configures the servo pin as output
}

void loop() {
  // Sets the servo position in degrees (between 0 and 180)
  moveServo(0); // central position
  delay(1000); // waits for 1 second

  moveServo(90); // minimum position
  delay(1000); // waits for 1 second

  moveServo(180); // maximum position
  delay(1000); // waits for 1 second
}

void moveServo(int angle) {
  int pulse_width_us = map(angle, 0, 180, 1000, 2000); // Maps the angle to pulse width in microseconds
  int pulse_period_us = 1000000 / PWM_FREQ; // Calculates the pulse period in microseconds
  int num_cycles = pulse_period_us / 1000; // Calculates the number of PWM cycles

  for (int i = 0; i < num_cycles; i++) {
    digitalWrite(SERVO_PIN, HIGH); // High voltage
    delayMicroseconds(pulse_width_us); // Pulse width
    digitalWrite(SERVO_PIN, LOW); // Low voltage
    delayMicroseconds(pulse_period_us - pulse_width_us); // Remaining space in the period
  }
}