/* A maze solving robot by Ben Axelrod
The robot uses the A* method to mark cells and determine shortest path. This program
is actually very small, however, the choose function is huge. This is because this
is where all of the robot's maze solving strategy is stored. This strategy is as
follows:
1. Check for walls (so you know where you absolutly can and can not go)
2. If a cell around you is 2 more than your current cell or 2 less than
your current cell, go there. This ensures that when the robot
finds a longer path, it will update it. Even if the path you it
is on needs the updating. Some of this is taken care of in the
main search task. (Even though the check for +2 and -2 is the same,
different things happen in each scenario. If another cell is -2,
then the robot moves onto it, records its index# then updates the
new path. If the cell is +2, the robot proceeds just like it was a
0 cell, overwriting the path's numbers.)
3. If there is a zero cell, go there. This keeps the robot finding new
paths.
4. If there is still more than one way to go, randomly choose one.
In all of these "checks" above, the robot will go straight first if it can. Hopefully,
these checks will sufficiently narrow down the choice of paths for the robot from
potentially 3 to 1 every time. If not there is a randomizer that will choose one path
from more than one. This isn't the most elagent code, but it works. Another neat check
that was not implemented here, might be to head in the general direction of the goal.
Also there is no "better path" checking in this program. Once the robot has found the
goal cell, it does not look to see if it has found all the paths leading to the goal.
It then takes the fastest path it knows of all the way back home. I tried to eliminate
this problem in the check function by having the robot always going over old paths and
going where there is a 0. That is, hopefully it will find the fastest path before it
finds the goal.
This program is just the brains of the robot. I have not yet spent time into a reliable
wall sensing method and cell positioning. I tested this robot with 3 touch sensors and
at every intersection I had to press the touch sensors where the robot could go. It was
also nessisary to consantly re-position the robot so that it was centered in the cells.
*/
#pragma init Initialize()
#define Rgoal 1 //the row of the goal cell
#define Cgoal 4 //the column of the goal cell
#define Rstart 4 //the row of the start cell
#define Cstart 0 //the column of the start cell
#define mazesize 5 //5 is the max!
#define backmark 25 //what to mark the cells on your way out
#define leftmotor OUT_C
#define rightmotor OUT_A
#define spintime 26
#define fwdtime 48
#define sum (paths[0]+paths[1]+paths[2])
#define Do180 {TurnLeft(); TurnLeft();}
#define Array2D(r,c) Array1D[mazesize*r+c] //2D to 1D mapping function
int Array1D[mazesize*mazesize],R,C,Dr,Dc;
/*
R and C are the current cell
values for R and C [R,C]
[0,0][0,1][0,2][0,3][0,4]
[1,0][1,1][1,2][1,3][1,4]
[2,0][2,1][2,2][2,3][2,4]
[3,0][3,1][3,2][3,3][3,4]
[4,0][4,1][4,2][4,3][4,4]
Dr and Dc are the direction the robot is facing
(Dr,Dc) | Compas
---------|---------
(-1,0) | North
(0,1) | East
(1,0) | South
(0,-1) | West
Updating R and C when changing cells
Forward:
R=Dr+R;
C=Dc+C;
Left:
temp=Dr;
Dr=(-1)*Dc;
Dc=temp;
Right:
temp=Dc;
Dc=(-1)*Dr;
Dr=temp;
To look at the contents of another cell around you:
current cell value: Array2D(R,C)
forward cell value: Array2D((R+Dr),(C+Dc))
left cell value: Array2D((R-Dc),(C+Dr))
right cell value: Array2D((R+Dc),(C-Dr))
*/
void Initialize()
{
SetPower(leftmotor + rightmotor,5);
SetSensor(SENSOR_1,SENSOR_TOUCH);
SetSensor(SENSOR_2,SENSOR_TOUCH);
SetSensor(SENSOR_3,SENSOR_TOUCH);
int x;
for (x=0;x=index)
{
Array2D(R,C)=index; //mark cell
index++; //increment index
}
else
{
if (Array2D(R,C) 1 )
{
//check2 go there if the cell is more than +- 2 !!!but not zero!!!
if (paths[1]==1 && Array2D((R+Dr),(C+Dc))!=0) //check fwd
if ( Array2D((R+Dr),(C+Dc))>Array2D(R,C)+1 || Array2D((R+Dr),(C+Dc))Array2D(R,C)+1 || Array2D((R-Dc),(C+Dr))Array2D(R,C)+1 || Array2D((R+Dc),(C-Dr))1)
{
//check 3 go there if there is a zero
if ( paths[1]==1 && Array2D((R+Dr),(C+Dc))==0 ) //check fwd
{
paths[0]=0;
paths[1]=1;
paths[2]=0;
}
if ( paths[0]==1 && Array2D((R-Dc),(C+Dr))==0 ) //check left
{
paths[0]=1;
paths[1]=0;
paths[2]=0;
}
if ( paths[2]==1 && Array2D((R+Dc),(C-Dr))==0 ) //check right
{
paths[0]=0;
paths[1]=0;
paths[2]=1;
}
if (sum>1) //check4 random time!
{
if (sum==3) // [1 1 1]
{
paths[0]=0;
paths[1]=0;
paths[2]=0;
paths[Random(2)]=1;
}
if ( paths[0]==1 && paths[1]==1) // [1 1 0]
{
if (Random(1))
{
paths[0]=1;
paths[1]=0;
paths[2]=0;
}
else
{
paths[0]=0;
paths[1]=1;
paths[2]=0;
}
}
if ( paths[0]==1 && paths[2]==1) // [1 0 1]
{
if (Random(1))
{
paths[0]=1;
paths[1]=0;
paths[2]=0;
}
else
{
paths[0]=0;
paths[1]=0;
paths[2]=1;
}
}
if ( paths[1]==1 && paths[2]==1 ) // [0 1 1]
{
if (Random(1))
{
paths[0]=0;
paths[1]=1;
paths[2]=0;
}
else
{
paths[0]=0;
paths[1]=0;
paths[2]=1;
}
}
}
}
}
if ( sum == 0 )
Do180;
if ( paths[0] == 1 )
TurnLeft();
else if ( paths[2] == 1 )
TurnRight();
}
void backtrack()
{
PlaySound(5);
int paths[3];
Do180;
Wait(50);
until (R==Rstart && C==Cstart)
{
Array2D(R,C)=Array2D(R,C)+backmark;
PlaySound(0);
paths[0]=0;
paths[1]=0;
paths[2]=0;
ClearTimer(0);
while(Timer(0)<20) //0 for wall, 1 for path
{
if (SENSOR_1 == 1)
paths[0]=1;
if (SENSOR_2 == 1)
paths[1]=1;
if (SENSOR_3 == 1)
paths[2]=1;
}
PlaySound(0);
Wait(10);
if ( Array2D((R-Dc),(C+Dr))==(Array2D(R,C)-backmark-1) && paths[0]==1)
TurnLeft();
else if ( Array2D((R+Dc),(C-Dr))==(Array2D(R,C)-backmark-1) && paths[2]==1)
TurnRight();
DriveFwd();
}
Array2D(R,C)=Array2D(R,C)+backmark;
PlaySound(5);
Do180;
Wait(100);
until (R==Rgoal && C==Cgoal)
{
if ( Array2D((R-Dc),(C+Dr))== Array2D(R,C)+1 )
TurnLeft();
else if ( Array2D((R+Dc),(C-Dr))== Array2D(R,C)+1 )
TurnRight();
DriveFwd();
Wait(25);
}
PlaySound(5);
}
void TurnLeft()
{
int temp;
temp=Dr;
Dr=(-1)*Dc;
Dc=temp;
OnRev(rightmotor);
OnRev(leftmotor);
Wait(spintime);
Off(rightmotor+leftmotor);
}
void TurnRight()
{
int temp;
temp=Dc;
Dc=(-1)*Dr;
Dr=temp;
OnFwd(rightmotor);
OnFwd(leftmotor);
Wait(spintime);
Off(rightmotor+leftmotor);
}
void DriveFwd()
{
R=Dr+R;
C=Dc+C;
OnFwd(leftmotor);
OnRev(rightmotor);
Wait(fwdtime);
Off(rightmotor);
Off(leftmotor);
}