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;
}
}
}
/************************************************************************/
