UART PIC Microcontroller + Servo Motor

Main

#include "Includes.h"
#include <htc.h>
#include "UART.h"
#define x_middle 1473
#define _XTAL_FREQ   4000000  
#define SetBit(x,y) (x|=(1<<y))
#define ClrBit(x,y) (x&=~(1<<y))
#define INTERVAL 2000
volatile unsigned int SetingServo;

unsigned int stepsize = 100; 
struct temp_type {  
unsigned char   Lo ;
unsigned char   Hi ;
}
volatile union{                           
struct temp_type Servobyte;       
unsigned int Servo;    
}tempSeting;



void interrupt timer0_isr(void)
{
	if (T0IF)

   {

          TMR0=100-TMR0;          	 // TMR0 256-156=100
          T0IF=0;                    // Fosc=4??? , ????????????=1:128 >> 1???*128*156=20??
         
         
          tempSeting.Servo= 65536 - SetingServo; // 
         
          T1CON=0x00;            // TMR1
           TMR1L= tempSeting.Servobyte.Lo;  
           TMR1H= tempSeting.Servobyte.Hi;                                                         
           TMR1IF=0;                                                       
           TMR1ON=1;                                                      
          
                 RB2=1;   
   }
if (TMR1IF)
          {
                 RB2=0;         
                 TMR1IF=0; 
                 TMR1ON=0;                              
           }


}
void delay(unsigned int p)
{
	unsigned int i;
	for(i=0;i<p;i++){asm("NOP");}
}
void main(void)
{
  
  INTCON = 0x00;
  TRISB = 0b00000011;
  PORTB = 0b00000011;
  TRISC = 0xFF;              // Set RC0 and RC1 as input others as Output
  ANSEL = 0x00;   
  ANSELH = 0x00;                 
  PORTC = 0xFF;
  InitUART();
  OPTION_REG = 0b01000110; // 1:128 Prescaller
  TMR0=156;                         // Interupt every 0.1 ms
  TMR1IE=1;                        //Enable interrupt on TMR1 overflow
  T0IE = 1;                       //Enable interrupt on TMR0 overflow
  PEIE = 1;                      //Enable  Peripheral Interrupt
  GIE = 1; 
  unsigned int y,m;  
  unsigned int n = 1473;   // Middle angle for servomotor  ( 0 )
  SetingServo = y;//1473 ;
  InitUART();
  unsigned char uart_read;
  unsigned char cmd;
while(1)
{
	if (UART_Data_Ready()) {     

      uart_read = getch();

      putch(uart_read);
						}
      cmd = getch();
	  dvijenie();
}
}
char dvijenie ()
	{
	while(getch()) continue;
    cmd = getch();
    SetBit(PORTB,6);			//	здесь сервопривод установлен
		delay(n);
		ClrBit(PORTB,6);			//	здесь сервопривод установлен
		delay(m);
	switch (cmd) 
			{	             
 			case 'l':
				if(x_middle<1900)				
				{ 
    			 y = n + stepsize;
         	 m=TMR0 - y;
	         delay(100);
				}
				  break;
					
				
			case 'r':		
				if(x_middle > 985)
 		          
				{ 
    			  y = n - stepsize;
	          m=TMR0 - y;
	          delay(100);
				} 
				  break;
			}
    }    
    
 

UART

#include <htc.h>
//#include "UART.h"
#ifndef _XTAL_FREQ
	#define _XTAL_FREQ   4000000  // Hz
#endif
#define BAUDRATE 9600  //bps
void InitUART(void)
{
	TRISC6 = 1;   					// TX Pin
	TRISC7 = 1;   					// RX Pin
	
	SPBRG = ((_XTAL_FREQ/16)/BAUDRATE) - 1;
	BRGH  = 1;                   	// Fast baudrate
	SYNC  = 0;						// Asynchronous
	SPEN  = 1;						// Enable serial port pins
	CREN  = 1;						// Enable reception
	SREN  = 0;						// No effect
	TXIE  = 0;						// Disable tx interrupts
	RCIE  = 1;						// Enable rx interrupts
	TX9   = 0;						// 8-bit transmission
	RX9   = 0;						// 8-bit reception
	TXEN  = 0;						// Reset transmitter
	TXEN  = 1;						// Enable the transmitter
}

void putch(unsigned char Byte)  // Writes a character to the serial port
{
	while(!TXIF);  // wait for previous transmission to finish
	TXREG = Byte;
}
char UART_Data_Ready()
{
  return RCIF;
}

unsigned char getch(void)   // Reads a character from the serial port
{
	if(OERR) // If over run error, then reset the receiver
	{
		CREN = 0;
		CREN = 1;
	}
	
	while(!RCIF);  // Wait for transmission to receive
	return RCREG;
}

void SendStringSerially(const unsigned char* st)
{
	while(*st)
		putch(*st++);
}