////////////////////////////declarations/////////////////////////////
  int i;
  int count = 0;
  int countb = 0;
  short int PWM_flag = 1;  //PWM_flag --->flag to set speed of motors

  void forward();
  void reverse();
  void stop();
  void fwdLeft();
  void fwdRight();
  void revLeft();
  void revRight();
  void left();
  void right();
  void manual();
  void obstacle();
/////////////////////////////////////////////////////////////////////
////////////////////////////Main function////////////////////////////
void main()
{
  Pwm_Init(2000);
  PORTB = 255;
  TRISB = 249;
  PORTC = 0;
  TRISC = 0;
  PORTD = 255;
  TRISD = 255;
  while(1)
  {
    while(PORTD!=15)
    {
      obstacle();
      count++;
      if(count == 15)
      {
        Pwm1_stop();
        Pwm2_stop();
        break;
      }
    }
    manual();
    count = 0;
  }
}
/////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////

////////////////*** Manual operator control routine ***//////////////
void manual()
{
  if((PORTB&0xF0)==16)          //forward
    forward();
  else if((PORTB&0xF0)== 32)    //reverse
    reverse();
  else if((PORTB&0xF0)== 48)    //toggle horn
    {
      PORTB.F1 = 0;
      delay_ms(250);
      PORTB.F1 = 1;
    }
  else if((PORTB&0xF0)== 64)    //sharp left
    left();
  else if((PORTB&0xF0)== 80)    //gradual forward left
    fwdLeft();
  else if((PORTB&0xF0)== 96)    //gradual reverse left
    revLeft();
  else if((PORTB&0xF0)== 112)   //Slower speed (PWM max duty = 80%)
    PWM_flag = 0;
  else if((PORTB&0xF0)== 128)   //sharp right
    right();
  else if((PORTB&0xF0)== 144)   //gradual forward right
    fwdRight();
  else if((PORTB&0xF0)== 160)   //gradual reverse right
    revRight();
  else if((PORTB&0xF0)== 176)   //high speed (PWM max duty = 100%)
    PWM_flag = 1;
  else                          //stop
    stop();
}
/////////////////////////end of manual control///////////////////////

//////////////////*** obstacle avoidance routine ***/////////////////
void obstacle()
{
   if((PORTD&0x0F)== 3)         //Obstacle behind
    {
      forward();
    }
   else if((PORTD&0x0F)== 7)    //Obstacle behind towards left
    {
       fwdLeft();
    }
   else if((PORTD&0x0F)== 11)   //Obstacle behind towards right
     {
       fwdRight();
     }
   else if((PORTD&0x0F)== 12)   //Obstacle in front
     {
       reverse();
     }
   else if((PORTD&0x0F)== 13)   //Obstacle in front towards right
     {
       revRight();
     }
   else if((PORTD&0x0F)== 14)   //Obstacle in front towards left
    {
      revLeft();
    }
   else                         //Obstacle blocking both front and behind
     {
       right();
       countb++;
       for(i=0;i<=7;i++)
       {
         PORTB.F1 = ~PORTB.F1;;
         delay_ms(250);
         if((i==2)||(i==6))
         {
           PORTB.F1 = 0;
           delay_ms(800);
         }
       }
       if(countb == 20)
       {
         stop();
         for(i=0;i<=99000;i++)
           manual();
         countb = 0;
       }
     }
}
//////////////////////end of obstacle avoidance//////////////////////


/////////////////////////////////////////////////////////////////////
/////////////////////*** Motor Control Routines ***//////////////////
void forward()    //function for forward motion
{
    if(PWM_flag == 1)
    {
      Pwm1_Change_Duty(255);
      Pwm2_Change_Duty(255);
    }
    else
    {
      Pwm1_Change_Duty(178);
      Pwm2_Change_Duty(178);
    }
    Pwm1_Start();
    Pwm2_Start();
    PORTC.F3 = 1;
    PORTC.F4 = 0;
    PORTC.F5 = 1;
    PORTC.F6 = 0;
}

void reverse()      //function for reverse motion
{
    if(PWM_flag == 1)
    {
      Pwm1_Change_Duty(255);
      Pwm2_Change_Duty(255);
    }
    else
    {
      Pwm1_Change_Duty(178);
      Pwm2_Change_Duty(178);
    }
    Pwm1_Start();
    Pwm2_Start();
    PORTC.F3 = 0;
    PORTC.F4 = 1;
    PORTC.F5 = 0;
    PORTC.F6 = 1;
}

void stop()       //function to stop
{
    Pwm1_Stop();
    Pwm2_Stop();
    PORTC.F3 = 0;
    PORTC.F4 = 0;
    PORTC.F5 = 0;
    PORTC.F6 = 0;
    PORTC.F1 = 0;
    PORTC.F2 = 0;
}

void fwdLeft()       //function for forward-left turning
{
    if(PWM_flag == 1)
    {
      Pwm1_Change_Duty(178);
      Pwm2_Change_Duty(255);
    }
    else
    {
      Pwm1_Change_Duty(127);
      Pwm2_Change_Duty(178);
    }
    Pwm1_Start();
    Pwm2_Start();
    PORTC.F3 = 1;
    PORTC.F4 = 0;
    PORTC.F5 = 1;
    PORTC.F6 = 0;
}

void fwdRight()        //function for forward-right turning
{
    if(PWM_flag == 1)
    {
      Pwm1_Change_Duty(255);
      Pwm2_Change_Duty(178);
    }
    else
    {
      Pwm1_Change_Duty(178);
      Pwm2_Change_Duty(127);
    }
    Pwm1_Start();
    Pwm2_Start();
    PORTC.F3 = 1;
    PORTC.F4 = 0;
    PORTC.F5 = 1;
    PORTC.F6 = 0;
}

void revLeft()         //function for reverse-left turning
{
    if(PWM_flag == 1)
    {
      Pwm1_Change_Duty(178);
      Pwm2_Change_Duty(255);
    }
    else
    {
      Pwm1_Change_Duty(127);
      Pwm2_Change_Duty(178);
    }
    Pwm1_Start();
    Pwm2_Start();
    PORTC.F3 = 0;
    PORTC.F4 = 1;
    PORTC.F5 = 0;
    PORTC.F6 = 1;
}

void revRight()        //function for reverse-right turning
{
    if(PWM_flag == 1)
    {
      Pwm1_Change_Duty(255);
      Pwm2_Change_Duty(178);
    }
    else
    {
      Pwm1_Change_Duty(178);
      Pwm2_Change_Duty(127);
    }
    Pwm1_Start();
    Pwm2_Start();
    PORTC.F3 = 0;
    PORTC.F4 = 1;
    PORTC.F5 = 0;
    PORTC.F6 = 1;
}

void left()        //function for sharp left turning
{
    if(PWM_flag == 1)
    {
      Pwm1_Change_Duty(255);
      Pwm2_Change_Duty(255);
    }
    else
    {
      Pwm1_Change_Duty(178);
      Pwm2_Change_Duty(178);
    }
    Pwm1_Start();
    Pwm2_Start();
    PORTC.F3 = 0;
    PORTC.F4 = 1;
    PORTC.F5 = 1;
    PORTC.F6 = 0;
}

void right()        //function for sharp right turning
{
    if(PWM_flag == 1)
    {
      Pwm1_Change_Duty(255);
      Pwm2_Change_Duty(255);
    }
    else
    {
      Pwm1_Change_Duty(178);
      Pwm2_Change_Duty(178);
    }
    Pwm1_Start();
    Pwm2_Start();
    PORTC.F3 = 1;
    PORTC.F4 = 0;
    PORTC.F5 = 0;
    PORTC.F6 = 1;
}
///////////////////////end of motor control routines/////////////////
/////////////////////////////////////////////////////////////////////


////////////////////////////END OF PROGRAM///////////////////////////