#include <avr/io.h>
#include <util/delay.h>

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

    TCCR1A = (1 << COM1A1) | (1 << WGM11);
    TCCR1B = (1 << WGM13) | (1 << WGM12) | (1 << CS11); // Prescaler = 8

    ICR1 = 40000; // 20ms period for 50Hz PWM
}

void set_servo_angle(uint8_t angle) {
    OCR1A = 2000 + ((uint32_t)angle * 2000) / 180;
}

int main(void) {
    timer1_init();

    while (1) {
        for (uint8_t angle = 0; angle <= 180; angle++) {
            set_servo_angle(angle);
            _delay_ms(15);
        }

        for (int8_t angle = 180; angle >= 0; angle--) {
            set_servo_angle(angle);
            _delay_ms(15);
        }
    }
}



/*
#include <Servo.h>

Servo myServo;  // Create servo object

void setup() {
  myServo.attach(9);  // Attach servo to pin 9
}

void loop() {
  // Sweep from 0 to 180
  for (int angle = 0; angle <= 180; angle++) {
    myServo.write(angle);
    delay(15);  // Wait for servo to reach the position
  }

  // Sweep from 180 to 0
  for (int angle = 180; angle >= 0; angle--) {
    myServo.write(angle);
    delay(15);
  }
}
*/