Question

C言語を使用して、ライントレースロボットを動かすことをしています。ライントレースロボットが走行するコースは白地に黒線の部分と黒地に白線のものがあります。黒線から白線へ移動するときに、一瞬何も線が引かれてない白地の部分を通ります。この動作をセンサーの白黒反転と呼んでいます。白線から黒線へ移るときはこれと逆になります。使用しているマイコンはpic16f84aで、mplabを使用しています。環境はWindows10です。以下に挙げるプログラムでは黒字に白線の部分では動きますが、白地に黒線の部分では動きません。また、センサーの白黒反転もできていません。以下のプログラムをどのように改善すればよいか教えてください。flagを使用している部分が、反転するプログラムです。
// PIC16F84A Configuration Bit Settings

// 'C' source line config statements

// CONFIG
#pragma config FOSC = HS        // Oscillator Selection bits (HS oscillator)
#pragma config WDTE = OFF       // Watchdog Timer (WDT disabled)
#pragma config PWRTE = OFF      // Power-up Timer Enable bit (Power-up Timer is disabled)
#pragma config CP = OFF         // Code Protection bit (Code protection disabled)

// #pragma config statements should precede project file includes.
// Use project enums instead of #define for ON and OFF.
#include <pic.h>
#define RIGHT_DOWN 1
#define LEFT_DOWN 2
#define STRAIGHT 0
//CONFIG(0xFFFA); /* ???? CP:OFF,PT:OFF,WT:OFF,HS */f
int flag=0;

wait00(short k)
{
/* ?(k×0.01)??? ?????(k×0.01msec wait)*/
    short i;
    short j; /* 16 Bit ????? */
       for(j=0;j<k;j++){ /* (k×2)?????? */
        for(i=0;i<2;i++){
        }
    }
    return 0;
}

left_down(void){
    PORTA=0x03; /* both motor on */
    wait00(3);   /* 0.03msec wait */
    PORTA=0x01; /* right motor on */
    wait00(18);   /* 0.18msec wait */
    PORTA=0x00; /* both motor off */
    wait00(73);   /* 0.79msec wait */
}

small_left_down(void){
    PORTA=0x03; /* both motor on */
    wait00(5);   /* 0.05msec wait */
    PORTA=0x01; /* right motor on */
    wait00(12);   /* 0.12msec wait */
    PORTA=0x00; /* both motor off */
    wait00(78);   /* 0.83msec wait */
}

right_down(void){
    PORTA=0x03; /* both motor on */
    wait00(3);   /* 0.03msec wait */
    PORTA=0x02; /* left motor on */
    wait00(18);   /* 0.18msec wait */
    PORTA=0x00; /* both motor off */
    wait00(73);   /* 0.79msec wait */
}

small_right_down(void){
    PORTA=0x03; /* both motor on */
    wait00(5);   /* 0.05msec wait */
    PORTA=0x02; /* left motor on */
    wait00(12);   /* 0.12msec wait */
    PORTA=0x00; /* both motor off */
    wait00(78);   /* 0.83msec wait */
}


straight(void){
    PORTA=0x03; /* both motor on */
    wait00(20);   /* 0.20msec wait */
    PORTA=0x00; /* both motor off */
    wait00(78);   /* 0.80msec wait */
}

led_sens(void)
{
    RB3=RB0; /* ??????LED?? */
    RB4=RB1; /* ??????LED?? */
    RB5=RB2; /* ??????LED?? */
}

reverse_check(void){
    led_sens();
        //check_BW();
    if(RB0==0 && RB1==0 && RB2==0){
        flag=1;
    }
    else if(RB0==1 && RB1==0 && RB2==1){
        flag=1;
    }
    else if(RB0==1 && RB1==1 && RB2==1){
        flag=0;
    }
    else if(RB0==0 && RB1==1 && RB2==0){
        flag=0;
    }
}

int main(void)
{
    TRISA = 0xFC; /* A 0,1:output, 2,3,4:input */
    TRISB = 0xC7; /* B0,1,2:input, B3,4,5:LEDoutput, other bits input */
    PORTB = 0;    /* PORTB clear */
    PORTA = 0;    /* PORTA clear */
    int last_time = STRAIGHT;
    
    while(RA4==1){
        led_sens();
    }
    wait00(30000);
            
    while(1){/* ????? */
        reverse_check();
        //check_BW();
         if(flag==0){
            if(RB0==0 && RB1==0 && RB2==0 && last_time==LEFT_DOWN){
                left_down(); //"White White White" and "LEFT_DOWN last time"
                last_time=LEFT_DOWN; //turn left
            }
            else if(RB0==0 && RB1==0 && RB2==0 && last_time==RIGHT_DOWN){
                right_down(); //"White White White" and "RIGHT_DOWN last time"
                last_time=RIGHT_DOWN; //turn right
            }
            else if(RB0==1 && RB1==1 && RB2==0){ //Black Black White
                small_left_down(); //turn left a little
                last_time=LEFT_DOWN;
            }
            else if(RB0==1 && RB1==0 && RB2==0){ //Black White White
                left_down(); //turn left
                last_time=LEFT_DOWN;
            }
            else if(RB0==0 && RB1==1 && RB2==1){ //White Black Black
                small_right_down(); //turn right a little
                last_time=RIGHT_DOWN;
            }
            else if(RB0==0 && RB1==1 && RB2==1){ //White Black Black
                small_right_down(); //turn right a little
                last_time=RIGHT_DOWN;
            }
            else{
                straight();
                last_time=STRAIGHT;
            }
        }
        else{
            if(RB0==1 && RB1==1 && RB2==1 && last_time==LEFT_DOWN){
                left_down(); //"White White White" and "LEFT_DOWN last time"
                last_time=LEFT_DOWN; //turn left
            }
            else if(RB0==1 && RB1==1 && RB2==1 && last_time==RIGHT_DOWN){
                right_down(); //"White White White" and "RIGHT_DOWN last time"
                last_time=RIGHT_DOWN; //turn right
            }
            else if(RB0==0 && RB1==0 && RB2==1){ //Black Black White
                small_left_down(); //turn left a little
                last_time=LEFT_DOWN;
            }
            else if(RB0==0 && RB1==1 && RB2==1){ //Black White White
                left_down(); //turn left
                last_time=LEFT_DOWN;
            }
            else if(RB0==1 && RB1==0 && RB2==0){ //White Black Black
                small_right_down(); //turn right a little
                last_time=RIGHT_DOWN;
            }
            else if(RB0==1 && RB1==1 && RB2==0){ //White White Black
                right_down(); //turn right
                last_time=RIGHT_DOWN;
            }
            else{ //The other case
                straight(); //go to straight
                last_time=STRAIGHT;
            }
        }
    }
    return 0;
}