Maze Solver Emulation
Download zip file of the program below
//--------------------------------------------------
//
// Maze Solver Emulation
// Frank Borowiec
// Atlanta Hobby Robot Club
// www.botlanta.com
//
// Maze solving based on 3pi code
// use with Robot Basic - www.robotbasic.org
//
// Revision History:
// 09/23/09 fmb V2.5 Initial release
//--------------------------------------------------
DIM a[20,3]
LineWidth 3
sPath_L="" \ sPath_R="" \MazeCourse = ""\dir=""\bPos=""
LeftSolve=0\RightSolve=0
MazeCourse="AHRC" // default maze slection
rstart=1 \ Rev_Run_Auto=0
//--------------------------------------------------
// Maze & Options Selection
// Main Loop
//--------------------------------------------------
gosub initButtons // set up menu
MainLoop:
getbutton b
if b = "Maze_A" \ MazeCourse="AHRC" \ gosub DrawMaze
elseif b = "Maze_B" \ MazeCourse="Home" \ gosub DrawMaze
elseif b = "Solve Left" \ Solve_dir="L" \sDir="Left" \gosub rSolve //Solve_Left
elseif b = "Solve Right" \ Solve_dir="R" \ sDir="Right" \gosub rSolve//goto Run_Maze
elseif b = "Run Left" \ Solve_dir="L" \sDir="Left"\gosub rRunit
elseif b = "Run Right" \ Solve_dir="R" \sDir="Right"\gosub rRunit
elseif b = "Auto Rev" \ gosub ToggleAutorev \
elseif b = "Turn Debug ON"\ Stepping On
elseif b = "Exit" \ exit
elseif b = "Maze Start Pos>" \ rstart=rstart+1 \ gosub ResetAndDrawMaze
elseif b == "<3 then return // can't simplify if < 3
if substring(rallDir,length(rallDir)-1,1) <> "B" then return // look for a turnaround, else leave
for x=0 to 2 // calculate simplified turn
ss=substring(rallDir,length(rallDir)-x,1) // where were we? add the total angle
if ss= "R" \ total_ang=total_ang+90
elseif ss= "L" \ total_ang=total_ang+270
elseif ss=="B" \ total_ang=total_ang+180
endif
next x
if total_ang=>360 then total_ang=total_ang-360 // sub 360 until <= 360
if total_ang=>360 then total_ang=total_ang-360 // might have to do 2x
ss=""
if total_ang= 0 then ss="S" // now substitute based on total angle
if total_ang= 90 then ss="R"
if total_ang=180 then ss="B"
if total_ang=270 then ss="L"
rallDir=left(rallDir,length(rallDir)-3) + ss // chop off the last 3 turns and add one
return
//--------------------------------------------------
// Follow Segment
//--------------------------------------------------
followseg:
while true
rforward 1
rs=rsense (0)
if (rs & 7) =0 then return // dead end
if (rs & 1) =1 then return // intersection
if (rs & 4) =4 then return // intersection
wend
return
//--------------------------------------------------
// Select Turn
//--------------------------------------------------
select_turn:
rDir=""
if Solve_dir="L" //For Left solve
if fnd_l=1 then rDir="L"\ return // checks sensors in order
if fnd_s=1 then rDir="S"\return
if fnd_r=1 then rDir="R"\return
rDir="B" \ return
else //For Right solve
if fnd_r=1 then rDir="R"\ return
if fnd_s=1 then rDir="S"\return
if fnd_l=1 then rDir="L"\return
rDir="B" \ return
endif
return
//--------------------------------------------------
// Bot Turn
//--------------------------------------------------
botTurn:
if rDir="L" then rTurn -90
if rDir="R" then rTurn 90
if rDir="B" then rTurn 180
return
//--------------------------------------------------
// Run a Solved Course
//--------------------------------------------------
rRunit:
if bPos="end" // see if bot at 'end'
if Rev_Run_Auto=1 then goto rRun_Rev // is Reverse is on then run reverse
endif
gosub DrawMaze \gosub Course_Status // redraw maze
xytext 10,100,"Running " + sDir +":","",9 // show what we are doing
if Solve_dir="L" \ sSolve=sPath_L
elseif Solve_dir="R" \ sSolve=sPath_R
endif
for x=1 to length(sSolve) // loop for each saved direction
gosub followseg // and follow segment
rforward 4 // go a bit more
rforward rDia-4 // go a bit more to clear the sensors
rDir=substring(sSolve,x,1) // which way next
gosub botTurn // go and make the turn
next x
bPos="end" // set then 'end ' var
return
//--------------------------------------------------
// Run a course reverse
//--------------------------------------------------
rRun_Rev:
xytext 10,100,"Running " + sDir +" Reverse","",9
if Solve_dir="L" \ sSolve=sPath_L // show what we are doing
elseif Solve_dir="R" \ sSolve=sPath_R
endif
rTurn 180 // turn around
for x= length(sSolve)-1 to 1 step -1 // set up loop
rDir_r=substring(sSolve,x,1) // get directions from last to first
gosub followseg // and follow segment
rforward 4 // go a bit more
rforward rDia-4 // go a bit more to clear the sensors
rDir=rDir_r
if rDir_r="L" then rDir="R"
if rDir_r="R" then rDir="L"
gosub botTurn // go and make the turn
next x
gosub followseg // need one more segment to end
bPos=""
return
//--------------------------------------------------
// Toggle AutoReverse Switch
//--------------------------------------------------
ToggleAutorev:
Rev_Run_Auto=abs(Rev_Run_Auto-1) \ xytext 680,115,"Disabled","",8
if Rev_Run_Auto=1 then xytext 680,115,"Enabled","",8
return
//--------------------------------------------------
// print Start circles
//--------------------------------------------------
Show_Start_circles:
gosub rStart
for x = 0 to 10
XI = offsx+a[x,0]*mzScale-mzScale/6
YI = offsy +a[x,1]*mzScale-mzScale/6
circleWH XI,YI,mzScale/3,mzScale/3,a[x,2]
next x
return
//--------------------------------------------------
// Select Course
//--------------------------------------------------
SelectCourse:
initButtons:
ClearScr
setColor black
MazeCourse="AHRC" // default maze slection
rstart=1
Rev_Run_Auto=0
gosub DrawMaze
addbutton "Maze_A",460,30,100 \ addbutton "Maze_B",570,30,100
addbutton "",570,70,100
addbutton "Solve Left",210,30,100 \ addbutton "Solve Right",210,70,100 \ addbutton "Auto Rev",570,110,100
addbutton "Run Left",320,30,100 \ addbutton "Run Right",320,70,100
addbutton "Turn Debug ON",460,150,100 \addbutton "Exit",570,150,100
//--------------------------------------------------
// Draw maze and Position Bot Sim on Start
//--------------------------------------------------
ResetAndDrawMaze:
sPath_L="" \ sPath_R=""
DrawMaze:
ClearScr
xyText 1,1, "Atlanta Hobby Robot Club Maze Simulation www.botlanta.com", "fs_Bold",11
bPos=""
if MazeCourse=="AHRC" then goto maze_AHRC
if MazeCourse=="Home" then goto maze_Home
if MazeCourse=="Add_one_Here" then goto maze_3 // next jump for maze
maze_AHRC:
offsx=40
offsy=300
org_x=0
org_y=0
mzScale = 40
a[0,0]=0 \ a[0,1]=0 \ a[0,2]=90
a[1,0]=5 \ a[1,1]=0 \ a[1,2]=180
a[2,0]=10 \ a[2,1]=0 \ a[2,2]=-90
a[3,0]=11 \ a[3,1]=3 \ a[3,2]=-90
a[4,0]=8 \ a[4,1]=3 \ a[4,2]=-90
a[5,0]=2 \ a[5,1]=5 \ a[5,2]=0
MaxStart_pos=5
maze = "rrrddlllururrdrruddrrruulldrurdrurlddrruudrudd"
XI = offsx+org_x*mzScale
YI = offsy +org_y*mzScale
DrawShape maze, XI, YI ,mzScale// ,red
org_x=2
org_y=1
maze = "ddllrrrdduulddulldru"
XI = offsx+org_x*mzScale
YI = offsy +org_y*mzScale
DrawShape maze, XI, YI ,mzScale// ,red
org_x=3
org_y=4
maze = "rruddruulrrrlu"
XI = offsx+org_x*mzScale
YI = offsy +org_y*mzScale
DrawShape maze, XI, YI ,mzScale// ,red
org_x=10
org_y=2
maze = "drlldlldrurdrurdruu"
XI = offsx+org_x*mzScale
YI = offsy +org_y*mzScale
DrawShape maze, XI, YI ,mzScale// ,red
org_x=12
org_y=2
//maze = "d"//"dldulldrurdrurdruu"
XI = offsx +org_x*mzScale -mzScale/6
YI = offsy +org_y*mzScale -mzScale/6
rectangleWH XI,YI,mzScale/3,mzScale/3,0,0
goto SetBotPosition
maze_Home:
a[0,0]=0 \ a[0,1]=0 \ a[0,2]=180
a[1,0]=4 \ a[1,1]=0 \ a[1,2]=180
a[2,0]=6 \ a[2,1]=0 \ a[2,2]=180
a[3,0]=7 \ a[3,1]=0 \ a[3,2]=180
a[4,0]=-1 \ a[4,1]=1 \ a[4,2]=180
a[5,0]=1 \ a[5,1]=1 \ a[5,2]=180
a[6,0]=2 \ a[6,1]=1 \ a[6,2]=90
a[7,0]=5 \ a[7,1]=1 \ a[7,2]=180
a[8,0]=-1 \ a[8,1]=2 \ a[8,2]=90
a[9,0]=2 \ a[9,1]=3 \ a[9,2]=-90
a[10,0]=3 \ a[10,1]=3 \ a[10,2]=0
MaxStart_pos=10
offsx=80
offsy=300
org_x=0
org_y=0
mzScale = 40
maze = "dlrrldlrdrrllurrrduulrruddrruudlrrudddllludrd"
maze1 = "dlrrldlrdrrllurrrduulrrud"
mazeA = "dlrrldlrdrrllurrrduulrruddDUu"
XI = offsx+org_x*mzScale
YI = offsy +org_y*mzScale
DrawShape maze, XI, YI ,mzScale ,0
org_x=5
org_y=4
XI = offsx +org_x*mzScale -mzScale/6
YI = offsy +org_y*mzScale -mzScale/6
rectangleWH XI,YI,mzScale/3,mzScale/3,0,0
goto SetBotPosition
maze_3:
// fill in maze 3 here
goto SetBotPosition
//goto SelLoop:
SetBotPosition:
rDia=16
if rstart>MaxStart_pos then rstart=0
if rstart<0 then rstart= MaxStart_pos
//aH=a[rstart,2]
rallDir=""
//XI = offsx+a[rstart,0]*mzScale//-mzScale/3
//Y/I = offsy +a[rstart,1]*mzScale//-mzScale/3
//aH=a[rstart,2]
//rLocate XI,YI,aH,rDia,2
//rpen up//down,green
//rInvisible green,black
sTxt="Maze " + MazeCourse \ xytext 680,35,sTxt,"",8
sTxt="Start @ : (" + a[rstart,0] +"," + a[rstart,1]+ ")" \ xytext 680,75,sTxt,"",8
sTxt="Disabled" \if Rev_Run_Auto=1 then sTxt="Enabled"
xytext 680,115,sTxt,"",8
setbotstartPos:
XI = offsx+a[rstart,0]*mzScale//-mzScale/3
YI = offsy +a[rstart,1]*mzScale//-mzScale/3
aH=a[rstart,2]
rLocate XI,YI,aH,rDia,2
rpen up//down,green
rInvisible green,black
return
//--------------------------------------------------
// Course Length Status
//--------------------------------------------------
Course_Status:
xyText 10,140, "Left = " +length(sPath_L) +", "+ sPath_L, "",9
xyText 10,160, "Right= " +length(sPath_R) +", "+ sPath_R, "",9
return
sTxt="Left Solve cnt="
print LeftSolve
if LeftSolve=0 then sTxt=sTxt + "???" else sTxt=sTxt+LeftSolve
xytext 10,30,sTxt,"",8
sTxt="Right Solve cnt="
sTxt=sTxt+RightSolve
xytext 10,50,sTxt,"",8
Maze " + MazeCourse \ xytext 680,35,sTxt,"",8
Previous page: Publications & Presentations by members
Next page: Contests