Source Code For Coordinate Robot

This is the field for for the robot. The robot will start at coordinate (0,0) and it will end at coordinate (0,2).

This robot has 4 sensors. SENS1 and SENS4 are used to detect junction. SENS2 and SENS3 are used to guide the robot along the line.

Below is the example of the code.You may adjust the code accordingly.

 //Grid Robot  

 #include <18f4550.h>
 #fuses HS,NOLVP,NOPROTECT,NOWDT
 #use delay (clock =20M)
 //!#define S1 PIN_B0 // PRESET BUTTON 1
 //!#define S2 PIN_B1 // PRESET BUTTON 2
 #define SENS1  PIN_D1 //Sensor1
 #define SENS2  PIN_D2 //Sensor2
 #define SENS3  PIN_D3 //Sensor3
 #define SENS4  PIN_D4 //Sensor4
 #define SENS5  PIN_D5 //Sensor5
 // DC MOTOR SPEED CONTROL //
 #define PWM_RIGHT  PIN_C2 // PWM OUTPUT 1
 #define PWM_LEFT   PIN_C1 // PWM OUTPUT 2
 #define FWD_LEFT   PIN_B2 // DC MOTOR FORWARD LEFT
 #define RVS_LEFT   PIN_B3 // DC MOTOR REVERSE LEFT
 #define FWD_RIGHT  PIN_B4 // DC MOTOR FORWARD LEFT
 #define RVS_RIGHT  PIN_B5 // DC MOTOR REVERSE LEFT
 //Directions of robots
 #define North 1
 #define South 2
 #define East  3
 #define West  4
 //Global variable
 int init_dir; //initial direction
 int pos_x, pos_y; //initial coordinate
 int grid_x, grid_y; //grid size
 //Function Prototype
 void forward(void);
 void left(void);
 void right();
 void reverse(void);
 void stop(void);
 void left_ninety();
 void right_ninety();
 void line();
 char coordinate();
 void search_pos();
 void overturn();
 void home();
 /************************************************************************/
 void main()
 {
   //Initial value for variables
   grid_x=8;
   grid_y=5;
   init_dir=1; //Initial direction North
   //Pin Configuration
   set_tris_b(0x00);//Set PORTB as output --> connect to motor driver
   set_tris_d(0xFF);//Set PORTD as input --> connect to sensor
   // PWM CONFIGURATION //
   setup_timer_2(T2_DIV_BY_4,254,1); //PWM 1 OUTPUT CONFIGURATION
   setup_ccp1(ccp_pwm); //PWM 1 DUTY CYCLE CONFIGURATION
   setup_ccp2(ccp_pwm);
   delay_ms(1000);
   search_pos();
   overturn();
   home();
   while(true);
 }
 /************************************************************************/
 void forward()
 {
   output_high(FWD_LEFT);
   output_low(RVS_LEFT);    
   output_high(FWD_RIGHT);  
   output_low(RVS_RIGHT);
   set_pwm1_duty(200);
   set_pwm2_duty(200);
   delay_ms(100);
 }
 /************************************************************************/
 void reverse()
 {
   output_low(FWD_LEFT);
   output_high(RVS_LEFT);    
   output_low(FWD_RIGHT);  
   output_high(RVS_RIGHT);  
   set_pwm1_duty(200);
   set_pwm2_duty(200);
   delay_ms(100);
 }
 /************************************************************************/
 void left()
 {
   output_high(FWD_LEFT);
   output_low(RVS_LEFT);    
   output_low(FWD_RIGHT);  
   output_low(RVS_RIGHT);
   set_pwm1_duty(100);
   set_pwm2_duty(100);
   delay_ms(100);
 }
 /************************************************************************/
 void right()
 {
   output_low(FWD_LEFT);
   output_low(RVS_LEFT);    
   output_high(FWD_RIGHT);  
   output_low(RVS_RIGHT);  
   set_pwm1_duty(100);
   set_pwm2_duty(100);
   delay_ms(100);
 }
 /************************************************************************/
 void stop()
 {
   output_low(FWD_LEFT);
   output_low(RVS_LEFT);    
   output_low(FWD_RIGHT);  
   output_low(RVS_RIGHT);  
   delay_ms(100);
 }
 /************************************************************************/
 void left_ninety()
 {
   forward();
   delay_ms(300);
   stop();
   //Hard Left Turn
   output_high(FWD_LEFT);
   output_low(RVS_LEFT);    
   output_low(FWD_RIGHT);  
   output_high(RVS_RIGHT);  
   set_pwm1_duty(100);
   set_pwm2_duty(100);
   delay_ms(400);
   left();
   delay_ms(100);
   while(SENS1==0); //while SENS1 didnt detect line
   stop();
   //Change direction of robot
   if(init_dir==North)
   {
    init_dir=West;
    return;
   }
   if(init_dir==West)
   {
    init_dir=South;
    return;
   }
   if(init_dir==South)
   {
    init_dir=East;
    return;
   }
   if(init_dir==East)
   {
    init_dir=North;
    return;
   }
 }
 /************************************************************************/
 void right_ninety()
 {
   forward();
   delay_ms(300);
   stop();
   //Hard Right Turn
   output_low(FWD_LEFT);
   output_high(RVS_LEFT);    
   output_high(FWD_RIGHT);  
   output_low(RVS_RIGHT);  
   delay_ms(400);
   right();
   delay_ms(100);
   while(SENS4==0); //while SENS4 didnt detect line
   stop();
   //Change direction of robot
   if(init_dir==North)
   {
    init_dir=East;
    return;
   }
   if(init_dir==East)
   {
    init_dir=South;
    return;
   }
   if(init_dir==South)
   {
    init_dir=West;
    return;
   }
   if(init_dir==West)
   {
    init_dir=North;
    return;
   }
 }
 /************************************************************************/
 void line()
 {
   if(!input(SENS2)&&!input(SENS3))
    forward();
   if(!input(SENS2)&&input(SENS3))
    left();
   if(input(SENS2)&&!input(SENS3))
    right();
 }
 /************************************************************************/
 char coordinate()
 {
   forward();
   delay_ms(100);
   while(true)
   {
    if(input(SENS1)||input(SENS4))
      break;
   }
   stop();
   if(init_dir==North)
   {
    pos_y++;
   }
   if(init_dir==South)
   {
    pos_y--;
   }
   if(init_dir==East)
   {
    pos_x++;
   }
   if(init_dir==West)
   {
    pos_x--;
   }
   return 0;
 }
 /************************************************************************/
 void search_pos()
 {
   pos_x=0;
   pos_y=0;
   while(true)
   {
 /************* First Target ****************/  
    while(pos_x<grid_x)
    {
      if(coordinate())
       return;
    }    
    if((pos_y==grid_y)&&(pos_x==0))
      return;    
    left_ninety();
 /************* Second Target ****************/    
    while(pos_y<5)
    {
      if(coordinate())
       return;
    }
    if((pos_y==grid_y)&&(pos_x==0))
      return;
    left_ninety();
  /************* Third Target ****************/    
    while(pos_x>3)
    {
      if(coordinate())
       return;
    }
    if((pos_y==grid_y)&&(pos_x==0))
      return;
    left_ninety();
 /************* Fourth Target ****************/    
    while(pos_y>2)
    {
      if(coordinate())
       return;
    }
    if((pos_y==grid_y)&&(pos_x==0))
      return;
    left_ninety();
 /************* Fifth Target ****************/    
    while(pos_x>0)
    {
      if(coordinate())
       return;
    }
    if((pos_y==grid_y)&&(pos_x==0))
      return;
    left_ninety();
   }
 }
 /************************************************************************/
 void overturn() //Robot rotate 180 degrees
 {
   //Hard Right Turn
   output_low(FWD_LEFT);
   output_high(RVS_LEFT);    
   output_high(FWD_RIGHT);  
   output_low(RVS_RIGHT);  
   delay_ms(900);  
   right();
   delay_ms(100);
   while(SENS4==0);
   stop();
   //Change direction of robot
   if(init_dir==North)
   {
    init_dir=South;
    return;
   }
   if(init_dir==East)
   {
    init_dir=West;
    return;
   }
   if(init_dir==South)
   {
    init_dir=North;
    return;
   }
   if(init_dir==West)
   {
    init_dir=East;
    return;
   }
 }
 /************************************************************************/
 void home()
 {
   coordinate();
   pos_y--;
   if(pos_x!=0)
   {
    left_ninety();
      while(pos_x)
       coordinate();
      return;
   }
   if(pos_x==0)
   {
    if(init_dir==North)
    {
      overturn();
      while(pos_y)
       coordinate();
      return;
    }
   }
 }
 /************************************************************************/