ULTRASONIC SENSOR MEASURED DISTANCE CONVERT TO SERVO MOTOR ANGLE IN MPLAB USING PIC 16F877A

can someone help me in creating code for this

the output of ultrasonic sensor converts to angle of the servo motor

i've tried to create a sample for this
can someone help me configuring this

#define _XTAL_FREQ 8000000

#include <xc.h>
#include <pic16f877a.h>

#pragma config FOSC = HS
#pragma config WDTE = OFF
#pragma config PWRTE = OFF
#pragma config BOREN = ON
#pragma config LVP = OFF
#pragma config CPD = OFF
#pragma config WRT = OFF

#pragma config CP = OFF


void servoRotate0()
{
 
    unsigned int i;
 
    for(i=0;i<50;i++)
 
{
   RB0 = 1;
  
    __delay_us(800);

    RB0 = 0;

    __delay_us(19200);
 
 }

}

void servoRotate180()
{
 
    unsigned int i;
 
    for(i=0;i<50;i++)
 
{
   RB0 = 1;
  
    __delay_us(2200);

    RB0 = 0;

    __delay_us(17800);
 
 }

}
void main()

{
 
int a;
TRISB = 0b00010000; 


T1CON = 0x10;


 while(1)
 

{
    TMR1H = 0;               

     TMR1L = 0;                 


     RB0 = 1;             
     __delay_us(10);             
     RB0 = 0;           
     while(!RB4);             
     TMR1ON = 1;             
     while(RB4);           
     TMR1ON = 0;             
     a = (TMR1L | (TMR1H<<8)); 
     a = a/58.82;               

     a = a + 1;


 if(a>=2 && a<=400)
{   servoRotate0();
    }
else
{
  servoRotate180();
    }
  }
}