Ayuda pic 18f4550

Que onda amigos, tengo un pequeño problema y es que programe 2 sensores ultrasonicos con un seguidor de linea, y logre programar cada uno por separado, mi problema fue al momento de juntarlo todo por mi configuracion solo me reconoce 1 sensor ultrasonico, solo pone el primero que mando a llamar en el programa principal, como puedo hacer que le haga caso a los 2?

Dejo el programa abajo
#FUSES XT,NOWDT,NOPROTECT,PUT,NOBROWNOUT,NOLVP
#use delay(clock=8000000)

float distancia, tiempo;
#define echo pin_B0
#define trig pin_B1
float distancia2, tiempo2;
#define echo2 pin_B2
#define trig2 pin_B3
#USE standard_io(b)
void ultra1()
{
if (distancia<20 && input(pin_c0)==1)
{
output_bit(pin_b4,1);
delay_ms(5);
}
else
{
output_bit(pin_b4,0);
delay_ms(5);
}
}
void ultra2()
{
if (distancia2<20 && input(pin_c0)==1)
{
output_bit(pin_b5,1);
delay_ms(5);
}
else
{
output_bit(pin_b5,0);
delay_ms(5);
}
}
void S1 ()
{
while(true)
{
output_high(trig);
delay_us(10);
output_low(trig);
while(!input(echo))
{}
set_timer1(0);
while(input(echo))
{}
tiempo=get_timer1();
distancia=(tiempo)/(29.15);
ultra1();
delay_ms(20);

}
}
void S2()
{
while(true)
{
output_high(trig2);
delay_us(10);
output_low(trig2);
while(!input(echo2))
{}
set_timer1(0);
while(input(echo2))
{}
tiempo2=get_timer1();
distancia2=(tiempo2)/(29.15);
ultra2();
ultra1();
delay_ms(20);

}
}
void main()
{
setup_timer_1(T1_INTERNAL|T1_DIV_BY_1);
delay_ms(50);
while (true)
{
while(true)
{
ultra1(); //aquí es donde no me funciona y si pongo primero ultra2 y s2 me reconoce al 2 y no al 1 y vieversa
S1()
ultra2();
S2();
}
}
}