#include <p18F4550.h>
#include <delays.h>

// Include this when using Bootloader Program ================================
#pragma udata

extern void _startup (void);        // See c018i.c in your C18 compiler dir
#pragma code _RESET_INTERRUPT_VECTOR = 0x000800
void _reset (void)
{
    _asm goto _startup _endasm
}
#pragma code

#pragma code _HIGH_INTERRUPT_VECTOR = 0x000808
void _high_ISR (void)
{
    ;
}

#pragma code _LOW_INTERRUPT_VECTOR = 0x000818
void _low_ISR (void)
{
    ;
}
#pragma code

#pragma code

//  Ends here ===============================================================

// Program starts here:
unsigned int Count;
int A=0;

void main(void)

{ ADCON1 = 0x0F;
 
 TRISB = 0b00000001;  // 1. Configure RB0 as echo input, RB1 as trigger output, RB3 as relay trigger
 TRISD = 0b00000000;	
  
 
 while(1) 
 {  
 Delay10KTCYx(120);//delay 50ms to prevent false echo
 
 
 // 2. pulse trigger signal, at least 10 us 
 PORTBbits.RB1 = 1; 
 Delay1KTCYx(1); // ~1000 x 4 x 1 / 48 us = 83.3 us 
 PORTBbits.RB1 = 0; 
 Delay1KTCYx(1); 
 
 // 3. measure echo pulse 
 while (PORTBbits.RB0 == 0); // wait here if echo signal not high 
 Count = 0; // echo back, start timing 
 while ((PORTBbits.RB0 == 1) && (Count < 30)) 
 { 
 Delay100TCYx (7); // delay 58 us 
 Count++;
 }// counts up if echo signal still high 
 
 // 4. Count now holds distance in cm, if obstacle is 200cm i.e. 2m or above assume obstacle not found  

 if (Count < 5) // obstacle very near
 { 
 PORTD=0b11111111;// All LEDs light
 
 }
 
 if (Count > 5 && Count < 10)
{
  PORTD=0b01111111; 
		}
 if (Count > 10 && Count < 14)
  {
  PORTD=0b00111111;
		}
 if (Count > 14 && Count < 17)
  {
  PORTD=0b00011111;
		}
 if (Count > 17 && Count < 20)
  {
  PORTD=0b00001111;
		}
 if (Count > 20 && Count < 23)
  {
  PORTD=0b00000111;
		}
 if (Count > 23 && Count < 27)
  {
  PORTD=0b00000011;
		}
 if (Count > 27 && Count < 30)
  {
  PORTD=0b00000001;
		}
 if (Count >30)
  {
  PORTD=0b00000000;
		}
  
  
  if (Count<5 && A==0)A=1;
  if (Count>7 && A==1)A=2;
  if (Count<5 && A==2)A=3;
  if (Count>7 && A==3)A=0;
  if (A==1)PORTBbits.RB2=1;
  if (A==2)PORTBbits.RB2=1;
  if (A==3)PORTBbits.RB2=0;
  

  
  
  
  

 } 
}