void setup() {
  // Set pin 9 (OC1A/PB1) as output
  DDRB |= (1 << PB1);

  // Clear Timer1 control registers
  TCCR1A = 0;
  TCCR1B = 0;

  // Pin Activation
  DDRB |= (1 << PB1);
  // Set Fast PWM mode using ICR1 as TOP value
  // WGM13:0 = 14 -> Fast PWM, ICR1 as TOP
  TCCR1A |= (1 << WGM11);
  TCCR1B |= (1 << WGM12) | (1 << WGM13);

  // Set non-inverting mode for OC1A (Pin 9)
  // COM1A1:0 = 2 -> Non-inverting mode
  TCCR1A |= (1 << COM1A1);

  // Set the prescaler to 8
  // CS12:0 = 2 -> Prescaler 8
  TCCR1B |= (1 << CS11);

  // Set ICR1 register to define the top value (frequency)
  // Example: 16MHz / (8 * 40000) = 50 Hz
  ICR1 = 40000;

  // Start with the servo at 0 degrees (1 ms pulse width)
  OCR1A = 2000;
}


void loop() {
  // Sweep from 0 to 180 degrees
  for (int i = 2000; i <= 4000; i++) {
    OCR1A = i;
  }

  delay(1000);
  // Sweep from 180 to 0 degrees
  for (int i = 4000; i >= 2000; i--) {
    OCR1A = i;
  }
  delay(1000);

}