UNO
I use a L298 Dual H-Bridge Motor Driver board to drive my motors
Just a common circuit ,arduino board connects to L298 Dual H-Bridge Motor Driver and a encoder connects to two external interrupt pins.
PWM pin5,6 they are belongs to timer0.
volatile byte rpmcount;
unsigned int rpm;
unsigned long timeold;
const int x_size = 10;
const int y_size = 5;
int i =0;
int m[x_size][y_size] ={{0,0,2,0,0},
{0,1,0,1,0},
{99,1,1,1,0},
{0,0,0,0,0},
{0,0,0,0,0},
{0,0,0,0,0},
{0,0,0,0,0},
{0,0,0,0,0},
{0,0,0,0,0},
{0,0,0,0,0}};
#define MotorR_I1 8
#define MotorR_I2 9
#define MotorL_I3 10
#define MotorL_I4 11
#define MotorR_ENA 5
#define MotorL_ENB 6
#define initial_speedr 100
#define initial_speedl 100
void setup() {
Serial.begin(9600);
pinMode(MotorR_I1,OUTPUT);
pinMode(MotorR_I2,OUTPUT);
pinMode(MotorL_I3,OUTPUT);
pinMode(MotorL_I4,OUTPUT);
pinMode(MotorR_ENA,OUTPUT);
pinMode(MotorL_ENB,OUTPUT);
attachInterrupt(0,rpm_fun,CHANGE);
attachInterrupt(1,rpm_fun2,CHANGE);
rpmcount = 0;
rpm = 0;
timeold = 0;
}
void advance(int d)
{ analogWrite(MotorR_ENA,initial_speedr);
analogWrite(MotorL_ENB,initial_speedl);
digitalWrite(MotorR_I1,LOW);
digitalWrite(MotorR_I2,HIGH);
digitalWrite(MotorL_I3,LOW);
digitalWrite(MotorL_I4,HIGH);
delay(d * 100);
}
void turnR(int d)
{
analogWrite(MotorR_ENA,initial_speedr);
analogWrite(MotorL_ENB,initial_speedl);
digitalWrite(MotorR_I1,HIGH);
digitalWrite(MotorR_I2,LOW);
digitalWrite(MotorL_I3,LOW);
digitalWrite(MotorL_I4,HIGH);
delay(d * 100);
}
void turnL(int d)
{ analogWrite(MotorR_ENA,initial_speedr);
analogWrite(MotorL_ENB,initial_speedl);
digitalWrite(MotorR_I1,LOW);
digitalWrite(MotorR_I2,HIGH);
digitalWrite(MotorL_I3,HIGH);
digitalWrite(MotorL_I4,LOW);
delay(d * 100);
}
void stopRL(int d)
{
digitalWrite(MotorR_I1,HIGH);
digitalWrite(MotorR_I2,HIGH);
digitalWrite(MotorL_I3,HIGH);
digitalWrite(MotorL_I4,HIGH);
delay(d * 100);
}
void moveForward(void)
{if(rpmcount!=0)rpmcount=0;
while(rpmcount<=48)
advance(0);
rpmcount=0;
stopRL(5);
}
void turnLeft90(void)
{if(rpmcount!=0)rpmcount=0;
while(rpmcount<=20)
turnL(0);
rpmcount=0;
stopRL(5);
}
void turnRight90(void)
{if(rpmcount!=0)rpmcount=0;
while(rpmcount<=20)
turnR(0);
rpmcount=0;
stopRL(5);
}
void update_map(void)
{
for(int x=0;x<x_size;x++)
{for(int y=0;y<y_size;y++)
{if(m[x][y]!=1&&m[x][y]!=99&& m[x][y]!=2)
m[x][y]=0;
}
}
}
void WavefrontSearch(void)
{
int goal_x, goal_y;
bool foundWave = true;
int currentWave = 2; //Looking for goal first
while(foundWave == true)
{
foundWave = false;
for(int y=0; y < y_size; y++)
{
for(int x=0; x < x_size; x++)
{
if(m[x][y] == currentWave)
{
foundWave = true;
goal_x = x;
goal_y = y;
if(goal_x > 0) //This code checks the array bounds heading WEST
if(m[goal_x-1][goal_y] == 0) //This code checks the WEST direction
m[goal_x-1][goal_y] = currentWave + 1;
if(goal_x < (x_size - 1)) //This code checks the array bounds heading EAST
if(m[goal_x+1][goal_y] == 0)//This code checks the EAST direction
m[goal_x+1][goal_y] = currentWave + 1;
if(goal_y > 0)//This code checks the array bounds heading SOUTH
if(m[goal_x][goal_y-1] == 0) //This code checks the SOUTH direction
m[goal_x][goal_y-1] = currentWave + 1;
if(goal_y < (y_size - 1))//This code checks the array bounds heading NORTH
if(m[goal_x][goal_y+1] == 0) //This code checks the NORTH direction
m[goal_x][goal_y+1] = currentWave + 1;
}
}
}
currentWave++;
delay(500);
}
}
void NavigateToGoal()
{
//Store our Robots Current Position
int robot_x, robot_y;
//First - Find Goal and Target Locations
for(int x=0; x < x_size; x++)
{
for(int y=0; y < y_size; y++)
{
if(m[x][y] == 99)
{
robot_x = x;
robot_y = y;
}
}
}
//Found Goal and Target, start deciding our next path
int current_x = robot_x;
int current_y = robot_y;
int current_facing = 0;
int next_Direction = 0;
int current_low = 99;
int Next_X = 0;
int Next_Y = 0;
while(current_low > 2)
{
current_low = 99; //Every time, reset to highest number (robot)
next_Direction = current_facing;
//Check Array Bounds West
if(current_x > 0)
if(m[current_x-1][current_y] < current_low && m[current_x-1][current_y] != 1) //Is current space occupied?
{
current_low = m[current_x-1][current_y]; //Set next number
next_Direction = 3; //Set Next Direction as West
Next_X = current_x-1;
Next_Y = current_y;
}
//Check Array Bounds East
if(current_x < (x_size -1))
if(m[current_x+1][current_y] < current_low && m[current_x+1][current_y] != 1) //Is current space occupied?
{
current_low = m[current_x+1][current_y]; //Set next number
next_Direction = 1; //Set Next Direction as East
Next_X = current_x+1;
Next_Y = current_y;
}
//Check Array Bounds South
if(current_y > 0)
if(m[current_x][current_y-1] < current_low && m[current_x][current_y-1] != 1)
{
current_low = m[current_x][current_y-1]; //Set next number
next_Direction = 2; //Set Next Direction as South
Next_X = current_x;
Next_Y = current_y-1;
}
//Check Array Bounds North
if(current_y < (y_size - 1))
if(m[current_x][current_y+1] < current_low && m[current_x][current_y+1] != 1) //Is current space occupied?
{
current_low = m[current_x][current_y+1]; //Set next number
next_Direction = 0; //Set Next Direction as North
Next_X = current_x;
Next_Y = current_y+1;
}
//Okay - We know the number we're heading for, the direction and the coordinates.
current_x = Next_X;
current_y = Next_Y;
//m[current_x][current_y] = '*';
//Track the robot's heading
while(current_facing != next_Direction)
{
if (current_facing > next_Direction)
{ if(current_facing==3&&next_Direction==0)
{
turnRight90();
current_facing=0;
}
else{
turnLeft90();
current_facing--;}
}
else if(current_facing < next_Direction)
{ if(current_facing==0&&next_Direction==3)
{
turnLeft90();
current_facing=3;
}
else{
turnRight90();
current_facing++;
}
}
}
moveForward();
delay(500);
}
int s;
s=m[robot_x][robot_y];
m[robot_x][robot_y]=m[current_x][current_y];
m[current_x][current_y]=s;
}
void loop() {
while(i==0){Serial.println("start tracing:");
WavefrontSearch();
NavigateToGoal();
stopRL(5);
update_map();
WavefrontSearch();
NavigateToGoal();
i=1;}
}
void rpm_fun()
{ //Serial.println( rpmcount);
rpmcount++;
}
void rpm_fun2()
{ //Serial.println( rpmcount);
rpmcount++;
}