#include  <18f2550.h>
#fuses XT, NOWDT, NOPROTECT,NOBROWNOUT,NOLVP,NOPUT,NOWRT,NODEBUG,NOCPD
        
#use delay(clock=20M)                      

#use fast_io(b)
#use fast_io(c)

#define cny1 pin_b0
#define cny2 pin_b1
#define cny3 pin_b2
#define cny4 pin_b3
#define cny5 pin_b4


   void main()
   {
  
   
   set_tris_c(0x000000000);
   set_tris_b(0xFF);
   
   output_c(0);
    
 begin:
   if(input(cny3) == 1 && input(cny4)== 1 && input(cny5) ==1 )   //SHARP RIGHT
        {
           output_high(pin_c7);
         output_low(pin_c6);
         
         output_high(pin_c5);
         output_low(pin_c4);
         
         delay_ms(20); 
          output_high(pin_c7);
         output_low(pin_c6);
         
         output_low(pin_c5);
         output_high(pin_c4);
         delay_ms(25);
         }
   
  else if(input(cny3) == 1 && input(cny4)== 1 && input(cny5) ==1 )   //SHARP LEFT
        {
           output_high(pin_c7);
         output_low(pin_c6);
         
         output_high(pin_c5);
         output_low(pin_c4);
         
         delay_ms(20); 
         output_high(pin_c5);
         output_low(pin_c4);
         
          output_low(pin_c7);
         output_high(pin_c6);

         delay_ms(25); 
         
        }
        
        else   if(input(cny2) == 0 && input(cny4) == 0)   // FORWARD
    {
   
          output_high(pin_c7);
         output_low(pin_c6);
         
         output_high(pin_c5);
         output_low(pin_c4);
         
         delay_us(20);
    }
   
    
   else  if(input(cny2) == 0 && input(cny4) == 1)   //right
    {
         output_high(pin_c7);
         output_low(pin_c6);
        
         output_low(pin_c5);
         output_high(pin_c4);
         
         delay_us(20);
    }
     else if(input(cny2) == 1 && input(cny4) == 0)   //left
    {
         output_low(pin_c7);
         output_high(pin_c6);
        
         output_high(pin_c5);
         output_low(pin_c4);
         
         delay_us(20);
    }
 
 

    goto begin;

   }