pic16f877a, Ultrasonido, ESP8266 y force sensor

#include <16f877.h>
#fuses  XT,NOWDT,NOPROTECT,NOLVP,NOBROWNOUT,PUT
#device adc=8
#use delay(clock=4Mhz)
#use RS232(BAUD=9600,XMIT=PIN_C6,RCV=PIN_C7,BITS=8,PARITY=N)
#use standard_io(a)
#use standard_io(b)
#use standard_io(c)
#use standard_io(d)

/*Environment Variables*/
/*Conexion Led Azul On*/
#define Led_Power pin_D4
/*Conexion UltraSonido*/
#define echo pin_B0
#define trig pin_B1
/*State Papel*/
#define Led_Green_p pin_B2
#define Led_Yellow_p pin_B3
#define Led_Red_p pin_B4
/*State Jabon*/
#define Led_Green_j pin_B5
#define Led_Yellow_j pin_B6
#define Led_Red_j pin_B7
/*TX_P_J_CON*/
#define Led_Green_tx_p pin_D7
#define Led_Green_tx_j pin_D6
#define Led_Green_tx_con pin_D5

/*Global Variables*/
/*Ultrasound variables*/
    float snd_time;
    float snd_distance;
    float snd_percentage;
/*Connection variables*/
float con_papel, con_jabon;
char con_rx;
/* Force sensor variables*/
    float fsr_reading;
    float fsr_force;
    float fsr_voltage;
    float fsr_resistance;
    float fsr_conductance;

void main(){
    /*System ON*/
    setup_timer_1(T1_INTERNAL|T1_DIV_BY_1);
    while(true){
    output_high(Led_Power);
    output_high(trig);                   
    delay_us(10);                       
    output_low(trig);
    while(!input(echo))                     
{}   
    set_timer1(0);
    while(input(echo))                     
{}
    /*Distancia del papel higienico*/
    snd_time = get_timer1();
    snd_distance =(snd_time/2)/(29.15);
    printf("%F",snd_distance);
    printf("\r");
    snd_percentage =(-23.25)*snd_distance +181.38;
    printf("%F",snd_percentage);
    printf("\r");
    if(snd_distance>=2.0 && snd_distance<=4.5){
        output_high(Led_Green_p);
        output_low(Led_Yellow_p);
        output_low(Led_Red_p);
    }
    if(snd_distance>=4.51 && snd_distance<=6.62){
        output_low(Led_Green_p);
        output_high(Led_Yellow_p);
        output_low(Led_Red_p);
    }
    if(snd_distance>=4.51 && snd_distance<=6.62){
        output_low(Led_Green_p);
        output_high(Led_Yellow_p);
        output_low(Led_Red_p);               
    }
    if(snd_distance>=6.63 && snd_distance<=10){
        output_low(Led_Green_p);
        output_low(Led_Yellow_p);
        output_high(Led_Red_p);
    }
    if (snd_distance >10.1){
        output_high(Led_Green_p);
        output_high(Led_Yellow_p);
        output_high(Led_Red_p);
    }
    con_papel=snd_distance;
   
        /*Puerto analogo*/
        SETUP_ADC(ADC_CLOCK_INTERNAL);
        SETUP_ADC_PORTS(AN0);
        SET_ADC_CHANNEL(0);
        delay_us(20);
        fsr_reading=READ_ADC();
        fsr_voltage=(5000*fsr_reading)/250;

        /*printf("Voltage reading in mV: ");
        printf("%F",fsr_voltage);
        printf(" mV");
        printf("\r");*/

        if(fsr_voltage == 0){
            output_high(Led_Red_j);
            fsr_force=0;
            /*printf("No pressure");
            printf("\r");*/
        }else{
            output_low(Led_Red_j);
            /*Calculo de la resistencia*/
            fsr_resistance = 5000 - fsr_voltage;
            fsr_resistance *= 10000;
            fsr_resistance /= fsr_voltage;
            /*printf("FSR Resistace in ohms: ");
            printf("%F",fsr_resistance);
            printf("\r");*/
            /*Calculo de la Conductancia*/
            fsr_conductance = 1000000;
            fsr_conductance /= fsr_resistance;
            /*printf("Conductance in microMhos: ");
            printf("%F",fsr_conductance);
            printf("\r");*/
   
            if(fsr_conductance <= 1000){
                /*Calculo de Force para peso del Jabon*/
                fsr_force = fsr_conductance /80;
                /*printf ("Force in Newtons: ");
                printf ("%F",fsr_force);
                printf("\r");*/
            }else{
                fsr_force = fsr_conductance -1000;
                fsr_force /= 30;
                fsr_force /=10;
                /*printf ("Force in Newtons: ");
                printf ("%F",fsr_force);
                printf("\r");*/
            }}
con_jabon=fsr_force;

    con_rx='e';
    /*Obtiene los datos del bufer del ESP*/
    con_rx=getc();
    /*Si no hay datos en el bufer como error enciende todos los leds de CON*/
    if(con_rx=='e'){
        output_high(Led_Green_tx_con);
        output_high(Led_Green_tx_j);
        output_high(Led_Green_tx_p);
    }
    if((con_rx=='j')){
        printf("%F",con_jabon);
        output_high(Led_Green_tx_j);
        delay_ms(250);
        output_low(Led_Green_tx_j);
        /*Envia datos al ESP*/
    }   
    if((con_rx=='p')){
        printf("%F",con_papel);
        output_high(Led_Green_tx_p);
        delay_ms(250);
        output_low(Led_Green_tx_p);
        /*Envia datos al ESP*/

    }}}