Manualni rizeni PWM

Napsal sem si funci na rizeni dvojice servomotoru HS 422
pouzity xtal = 14745600Hz
zapis je nasledujici:

[code]
int manual(void)
{
int prepocetH=90;
int prepocetL=90;
char info1[20];
char info2[20];
OCR3A=1500;
OCR3B=1500;
clear_disp();
sprintf(info1, “Servo 1: %d°”,prepocetH);
sprintf(info2, “Servo 2: %d°”,prepocetL);

		put_string_xy(0, 0, info1);
		cursor_disp_ctrl(0x04);
		put_string_xy(0, 1, info2);

key = 0;
while (key == 0);

// display button name
while(1) { 	
		   
	if (key != 0) {
		switch (key) {
			case 0x11 : {prepocetH+=1;}; break;		// 1
			case 0x12 : {prepocetH+=10;}; break;		// 2
			case 0x14 : {prepocetH+=100;}; break;		// 3
			case 0x21 : {prepocetH-=1;}; break;		// 4
			case 0x22 : {prepocetH-=10;}; break;		// 5
			case 0x24 : {prepocetH-=100;}; break;		// 6
			case 0x41 : {prepocetL+=1;}; break;		// 7
			case 0x42 : {prepocetL+=10;}; break;		// 8
			case 0x44 : {prepocetL+=100;}; break;		// 9
			case 0x82 : {prepocetL-=10;}; break;		// 0
			case 0x81 : {prepocetL-=1;}; break;		// *
			case 0x84 : {prepocetL-=100;}; break;		// #
			case 0x18 :return(1);break; 						// ESC
			
					}

		//pipani
		key = 0;
		beep(200, 10);

		if(prepocetH>=180) prepocetH=180;
		if(prepocetL>=180) prepocetL=180;
		if(prepocetH<=0) prepocetH=0;
		if(prepocetL<=0) prepocetL=0;

		//prepocet uhlu na ciselnou hodnotu OCR3A
		OCR3A=1000+(prepocetH*5.5);
		OCR3B=1000+(prepocetL*5.5);
		
		clear_disp();
		sprintf(info1, "Servo 1:  %d°",prepocetH);
		sprintf(info2, "Servo 2:  %d°",prepocetL);

		put_string_xy(0, 0, info1);
		cursor_disp_ctrl(0x04);
		put_string_xy(0, 1, info2);

		PWM();
				}
}

}

int PWM(void)
{
DDRE = 0x18; //povoleni portu PE3 a PE4 jako vystup PWM (OC3A a OC3B)
//Pouzita preddelicka 8 (prescaler)
ICR3=36864; //nastavenim ICR3=36864 dosahnu f=50Hz f=fxtal/N*TOP N=8 ICR1=TOP

TCCR3A|=(0<<COM3A0)|(1<<COM3A1)|(0<<COM3B0)|(1<<COM3B1)|(1<<WGM31)|(0<<WGM30); // fazove korigovana PWM (0<<WGM31)|(1<<WGM30)

//######################PWM PRO SERVO 2 OC1B################################

//Pouzita preddelicka 8 (prescaler)
ICR3=36864; //nastavenim ICR3=36864 dosahnu f=50Hz f=fxtal/N*TOP N=8 ICR1=TOP

TCCR3B|=(0<<ICNC3)|(0<<ICES3)|(1<<WGM33)|(1<<WGM32)|(0<<CS32)|(1<<CS31)|(0<<CS30);
}[/code]

Ale nejak mi nechce jit //prepocet uhlu na ciselnou hodnotu OCR3A OCR3A=1000+(prepocetH*5.5); OCR3B=1000+(prepocetL*5.5);

a tim padem mi nechtej jit serva respektive chcou se hybat ale snazi se najet nekam kam nemuzou co s tim?