Help in MPLAB programming for line following

Hi.... I got this LabView code for controlling a mobile robot with stepper motors...

I am adding 3 sensors on the robot for line following... the PIC im using is PIC16F877A and the sensors are connected to pins RB0, RB4 and RB5.... Can anyone help me in modifying the program?? Urgently in need of help pls...

This is the code...

//****************************************************************
//* *
//* *
//* *
//* *
//* Notes : This is a RS232 control of the MiniRobot *
//* : Stepper control is via Clock/Direction/Enable *
//* : Data sent as 3 byte serial control *
//****************************************************************

/*
The serial comms as implemented has a three byte length.
Control / Data 1/ Data 2 .
Data consists of 15bit (32767) info max. Neither MSB shall be set.
The MSB of data 2 is tranfered to the control byte for decoding.
Control Byte per bit is
MSB 7 = always 1 (control byte detection)
bit 6 = bit 1 of address
bit 5 = bit 0 of address
bit 4 = MSB of data2 byte
bit 3 = Left Motor Enabled
bit 2 = Right Motor Enabled
bit 1 = Left Motor Direction (1 forward / 0 reverse)
bit 0 = Right Motor Direction (1 forward / 0 reverse)

Left Right Left Right
Enable Enable Direct Direct
0 0 0 0 = 0 Brake
0 0 0 1 = 1 Stop
0 0 1 0 = 2 Stop
0 0 1 1 = 3 Stop
0 1 0 0 = 4 Left Reverse
0 1 0 1 = 5 Left Forward
0 1 1 0 = 6 Left Reverse
0 1 1 1 = 7 Left Forward
1 0 0 0 = 8 Right Reverse
1 0 0 1 = 9 Right Forward
1 0 1 0 = 10 Right Reverse
1 0 1 1 = 11 Right Forward
1 1 0 0 = 12 Reverse
1 1 0 1 = 13 Anticlockwise
1 1 1 0 = 14 Clockwise
1 1 1 1 = 15 Forward
*/

#include <pic.h>

#define XTAL 20000000 // Oscilator Frequency

static bit LeftPulse@ (unsigned)&PORTC*8+0;
static bit LeftDirection@ (unsigned)&PORTC*8+1;
static bit LeftEnable@ (unsigned)&PORTC*8+2;
static bit RightPulse@ (unsigned)&PORTC*8+3;
static bit RightDirection@ (unsigned)&PORTC*8+4;
static bit RightEnable@ (unsigned)&PORTC*8+5;
static bit LowPowerLED@(unsigned)&PORTB*8+1;

// Storage for general varibles
unsigned char Clk_Period; // timer 2 period
unsigned int TMR2_Overflow_Counter; // 500hz clock
unsigned int Stepping_Rate; // 100 = 200Hz
unsigned int Total_Steps; // input data (steps to achieve)
int Min_Pulse_Rate=50; // 100 = 200Hz
unsigned int Next_step; // flag and counter of stepps
char Next_Command; // command string
unsigned int Motor_data; // Input data for motors
char Next_Direction; // direction of next travel
char Left_M_State; // present enable state of left motor
char Right_M_State; // present enable state of right motor
char Address; // micro address
int Speeds[11]; // speed data
// Communication info next
volatile char Comm_Data_Rec; // flag for comms data recieved
char Command_Str; // first data byte
char Comm_Str1; // 2nd byte
char Comm_Str2; // 3rd byte
char data_Out; // comms data Output
char Rec_state=0; // postion in input string
int temp; // for tempory data varible
int ADC_Data; // Battery condition not inplemented yet

void SetUpPic(void); // Set Ports etc
void Set_base_address(void); // set base address
void Run_Motor(char c); // Run (1) / Stop (0) stepping
void Decode_Input_Data(void); // set new position data
char Set_Motor_State(void); // set Direction / Enable state for each motor
void Send_Message(char command, unsigned int Msg_Data); // send output
void Emergency_Stop(char Reason); // Stop movement and report error

// Start of main programme
void main(void)
{
SetUpPic();
Set_base_address(); // set base address
LeftEnable=0;
RightEnable=0; // turn off motors
LeftPulse=1;RightPulse=1; // clock pulse at start (high)
RightDirection=1;LeftDirection=1; // default is foward
Next_step=0; // flag if stepping
Address=2; // 877 has address 2
Speeds[0]=0;Speeds[1]=140;Speeds[2]=130;Speeds[3]=120;Speeds[4]=110;
Speeds[5]=100;Speeds[6]=90;Speeds[7]=80;Speeds[8]=70;Speeds[9]=60;Speeds[10]=50;
while(1) // Main loop for continues operation
{
// if (!Sys_Ok) // emergency stop switch check
// Emergency_Stop(3); // Stop movement and report error
if (Comm_Data_Rec) // have new data
{
Decode_Input_Data(); // decode new data
Comm_Data_Rec=0; // reset flag
Stepping_Rate = Speeds[Motor_data]; // get speed from array
if (Stepping_Rate<=Min_Pulse_Rate) // fastest revs
Stepping_Rate = Min_Pulse_Rate;
Run_Motor(1);
} // end new data
} // end of while loop
} // end of main

void Set_base_address(void) // set base address
{
// static bit Addy_0 @ (unsigned)&PORTB*8+1 ; // base address bit0
// static bit Addy_1 @ (unsigned)&PORTB*8+2 ; // base address bit1
// Address= (Addy_1*2)+ Addy_0;
Address=2; // use 1 for 873 and 2 for *877A
}

void Run_Motor(char Run) // Run (1) / Stop (0) stepping
{
if (Run) //note run command aslo includes comms stop.
{
TMR2_Overflow_Counter=0; // set for next run
Next_step=Set_Motor_State(); // enable and set direction of motors
}
else // stop
{
Next_step=0; // disable stepping first
// LeftEnable=0;RightEnable=0; // turn off motor controller
// LeftPulse=1;RightPulse=1; // clock pulse at start (high)
}
}

char Set_Motor_State(void)
{
char Run_Stop=0;
LeftEnable=0;
RightEnable=0; // ensure controllers are off
switch (Next_Command)
{
case 0-3:
LeftDirection=1; RightDirection=1; // default is foward
Left_M_State=0;Right_M_State=0; // ensure motors are OFF
LeftEnable=0;
RightEnable=0; // turn off motor
break;
case 4:
LeftDirection=1; RightDirection=1; // Left Reverse
Left_M_State=1;Right_M_State=0; // right motor is ON only
Run_Stop=1;
break;
case 5:
LeftDirection=0; RightDirection=0; // Left Forward
Left_M_State=1;Right_M_State=0; // right motor is ON only
Run_Stop=1;
break;
case 6:
LeftDirection=0; RightDirection=1; // Left Reverse
Left_M_State=0;Right_M_State=1; // right motor is ON only
Run_Stop=1;
break;
case 7:
LeftDirection=1; RightDirection=1; // Left Forward
Left_M_State=0;Right_M_State=1; // right motor is ON only
Run_Stop=1;
break;
case 8:
LeftDirection=1; RightDirection=1; // Right Reverse
Left_M_State=0;Right_M_State=1; // left motor is ON only
Run_Stop=1;
break;
case 9:
LeftDirection=1; RightDirection=0; // Right Forward
Left_M_State=1;Right_M_State=0; // left motor is ON only
Run_Stop=1;
break;
case 10:
LeftDirection=0; RightDirection=1; // Right Reverse
Left_M_State=1;Right_M_State=0; // left motor is ON only
Run_Stop=1;
break;
case 11:
LeftDirection=0; RightDirection=0; // Right Forward
Left_M_State=0;Right_M_State=1; // right motor is ON only
Run_Stop=1;
break;
case 12:
LeftDirection=1; RightDirection=1; // Reverse
Left_M_State=1;Right_M_State=1; // both motors are ON
Run_Stop=1;
break;
case 13:
LeftDirection=0; RightDirection=1; // Anticlockwise
Left_M_State=1;Right_M_State=1; // both motors are ON
Run_Stop=1;
break;
case 14:
LeftDirection=1; RightDirection=0; // Clockwise
Left_M_State=1;Right_M_State=1; // both motors are ON
Run_Stop=1;
break;
case 15:
LeftDirection=0; RightDirection=0; // Forward
Left_M_State=1;Right_M_State=1; // both motors are ON
Run_Stop=1;
break;
} // end switch

return Run_Stop;

} // end Set_Motor_State(void)

// Seperate 3Byte Comm Data
void Decode_Input_Data(void) // seperate new data
{
Next_Command=(Command_Str & 0b00001111); // Motor Enabled / Direction data
if (Command_Str & 0b00010000) // test bit4 data for MSB byte1
Comm_Str2 = Comm_Str2 + 0b10000000; // Add MSB
Motor_data= (Comm_Str1*256)+Comm_Str2; // combine data
}

void Send_Message(char command, unsigned int Msg_Data) // send output
{
char Out_Str1;
char Out_Str2;
command= command+128; // set MSB
Out_Str1=Msg_Data;
Out_Str2=Msg_Data>>8;
if (Out_Str1 & 0b10000000) // check for MSB set
{
command=command+16; // add MSB to command byte bit 4
Out_Str1 = Out_Str1 & 0b01111111;
}
if (Out_Str2 & 0b10000000) // check for MSB set
Out_Str2 = Out_Str2 & 0b01111111; // remove MSB
while(!TRMT){ // wait until buffer empty
}
TXREG=command; // write command character
while(!TRMT){ // wait until buffer empty
}
TXREG=Out_Str2; // write upper data
while(!TRMT){ // wait until buffer empty
}
TXREG=Out_Str1; // write lower data
}

// Emergency stop
// Reason (only limit (3) inplemented)
void Emergency_Stop(char Reason) // Stop movement and report error
{
temp = Next_step; // save present position
Next_step=0; // reset
Run_Motor(0); // stop steppers
Send_Message(Reason,temp); // send output
}
void SetUpPic(void)
{
// Port pin directions
TRISB = 0b00001111; // set RB4-7 output and RB0-3 to inputs
TRISC = 0b10000000; // output C0-5 , 6-7 comms

// Comm Port set at 9600
BRGH = 1; /* high baud rate */
SPBRG = 129; /* baud rate 9600 @ 20MHz*/

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 = 1; /* enable the transmitter */

ADCON0 = 0xC1; // enable ADC, RC osc. channel 0
ADCON1 = 0b00001110; // left justified, analog ch0,
// setup timer 2
Clk_Period =0xF9;
PR2= Clk_Period; // set timer 2 period = Clk_Period =0xF9
TMR2ON=1; // Turn on timer
// SET UP INTERUPTS
// INTCON bits = GIE,PEIE,T0IE,INTE,RBIE,T0IF,INTF,RBIF
OPTION=0;
TMR2IE=1; // TMR2 interrupt enable
// INTEDG = 0; // falling edge triggered PortB.0 (Bumper)
PEIE = 1; // Enable Peripheral Inerrupt
GIE = 1; // Enable Global Inerrupt
}

#pragma interrupt_level 0
static void interrupt ISR(void)
{
if (TMR2IF) // Timer 2 interrupt
{
if (Next_step) // do if stepping
{

if (TMR2_Overflow_Counter==0) // do at start of cycle
{
LeftPulse=0;RightPulse=0; // start next pulse
LeftEnable=Left_M_State;
RightEnable=Right_M_State; // enable controller
}

if (TMR2_Overflow_Counter==10) // clock length
{
LeftPulse=1;RightPulse=1; // motor step now
}
if (TMR2_Overflow_Counter==20) // Enable pulse length
{
// LeftEnable=0;
// RightEnable=0; // turn off controller
}

TMR2_Overflow_Counter++; // increment overflow counter
if (TMR2_Overflow_Counter>=Stepping_Rate) // ready for next step
{
TMR2_Overflow_Counter=0; // reset counter
} // end of start pulse
} // end next step
TMR2IF = 0; // reset timer 2 flag (50uS)
} // end TM2IF
if (RCIF) { // Recieved RS232 data
if (RCREG >= 128) // test for command byte
{
Command_Str=RCREG;
Command_Str=((Command_Str & 0b01100000)>>5); // get address
if (Command_Str==Address) // data for this micro
Rec_state=0; // move to next state
else // not for this base
Rec_state=3; // ignore to next command
if (Command_Str==0) // check for ALL address
Rec_state=0;
}
switch (Rec_state) {
case 0: // detect header at start of message
if (RCREG >= 128 ) // MSB set for command data
{
Command_Str=RCREG;
Rec_state=1; // move to next state
}
break;
case 1:
Comm_Str1 = RCREG; // 2nd byte
Rec_state=2; // move to next state
break;
case 2:
Comm_Str2 = RCREG; // 3rd byte
Rec_state=0; // reset for next message
Comm_Data_Rec =1; // flag new data message
break;
case 3: // wait till next command string
break;
}
RCIF=0;
} // end comms
}