Servomotor de control con microcontrolador ATmega328p

Durante un tiempo, he estado trabajando con Arduino, pero hace aproximadamente un mes, cambié a usar únicamente el microcontrolador ATmega328-PU. Yo uso el IDE de Atmel Studio. Hasta ahora, he hecho parpadear con éxito un LED, he usado programas que involucran botones/interruptores y he usado la función Fast PWM para atenuar un LED. Actualmente, estoy intentando conducir un servomotor usando Fast PWM, pero he tenido poco éxito. No he encontrado ningún código fuente que funcione para las necesidades específicas de mi microcontrolador, y realmente no tengo la capacidad de cambiar todos los parámetros, ya que todavía estoy aprendiendo.

El siguiente es el código que he desarrollado. No funciona, pero esperaba que alguien pudiera arreglarlo. El servo se mueve en una dirección, pero no se detiene y simplemente termina moliendo el engranaje del eje del motor, lo que indica que esto podría ser un problema con la longitud de onda. Una pregunta adicional: ¿existe una ecuación para convertir grados a longitud de onda PWM? ¡Gracias!

Especificaciones del microcontrolador:

• Modelo - ATmega328-PU
• cristal de 16.000.000 hz
• 28 pines
• Usando un temporizador de 8 bits

servo:

• Servomodelo: Torre Pro SG90
• Servofrecuencia 50 hz (¿esto es estándar?)

Código:

#define F_CPU 16000000UL
#include <avr/io.h>
#include <util/delay.h>

int main(void)
{

    DDRD |= (1 << DDD6);
    // PD6 is now an output - corresponds to OCR0A

    TCCR0A |= (1 << COM0A1);
    // set non-inverting mode

    TCCR0A |= (1 << WGM01) | (1 << WGM00);
    // set fast PWM Mode

    ICR1 = 39999;

    OCR0A = ICR1 - 2000;
    // gives servo motor position

    TCCR0B |= (1 << CS01);
    // set prescaler to 8 and starts PWM


    while(1)
    {
    }
}