Wow, that turned out to be way harder than I expected. It is very hard to do cleanly on the AVR timer. But I was able to come up with this working function, which I think is what you are looking for...
// freq_hz is frequency in hertz from 122 to 4,000,000
// shift_deg is phase shift between output A and output B in degrees (values outside -180 to +180 are ...
You have two problems
Both of them is related to your power supply :
1st, max. Voltage requirement for your servomotor is 6 volts, not 9 volts
2nd، your batteries are weaker than servo's need.
Your servomotor need 2.5 Amps. In the max. Torque.
So, over-voltage and not enough current is your fault
(According to 996R servomotor datasheet).
Change your power ...