Final Project

 

%prepare the planner for a new graph
laser "resetplanner"
laser "scanget"
laser "resetlocalizer"
%----------------------------------------------
%add nodes to the graph
laser "addpoint pno=1 x=0.5 y=0.5"
laser "addpoint pno=2 x=-0.5 y=1.5"
laser "addpoint pno=3 x=0.5 y=1.5"
laser "addpoint pno=4 x=3.5 y=1.5"
laser "addpoint pno=5 x=4.5 y=1.5"
laser "addpoint pno=6 x=-0.5 y=3.5"
laser "addpoint pno=7 x=0.5 y=3.5"
laser "addpoint pno=8 x=3.6 y=3.5"
laser "addpoint pno=9 x=4.5 y=3.5"
laser "addpoint pno=10 x=0.5 y=4.6"
laser "addpoint pno=11 x=3.6 y=4.6"
laser "addpoint pno=12 x=1.85 y=3.4"
laser "addpoint pno=13 x=2.25 y=3.4"
laser "addpoint pno=14 x=-0.5 y=2.5"
laser "addpoint pno=15 x=0.5 y=2.2"
laser "addpoint pno=16 x=3.6 y=2.5"
laser "addpoint pno=17 x=4.5 y=1.9"
laser "addpoint pno=18 x=4.5 y=3"
laser "addpoint pno=19 x=2 y=2.5"
laser "addpoint pno=20 x=2 y=3"
%laser "addpoint pno=21 x=2.05 y=3.3"
laser "addpoint pno=22 x=1.3 y=3.45"
laser "addpoint pno=23 x=2.7 y=3.45"
laser "addpoint pno=24 x=1.3 y=4"
laser "addpoint pno=25 x=2.7 y=4"
laser "addpoint pno=26 x=1.7 y=2.2"
laser "addpoint pno=27 x=2.2 y=2.2"
laser "addpoint pno=28 x=1.5 y=2.2"
laser "addpoint pno=29 x=2.5 y=2.2"
laser "addpoint pno=30 x=1.5 y=0.5"
laser "addpoint pno=31 x=2.5 y=0.65"

%----------------------------------------------
%add connections between points
laser "addcon pno1=1 pno2=3"
laser "addcon pno1=3 pno2=1"

laser "addcon pno1=3 pno2=15"
laser "addcon pno1=15 pno2=3"

laser "addcon pno1=2 pno2=14"
laser "addcon pno1=14 pno2=2"

laser "addcon pno1=6 pno2=14"
laser "addcon pno1=14 pno2=6"

laser "addcon pno1=15 pno2=14"
laser "addcon pno1=14 pno2=15"

laser "addcon pno1=7 pno2=15"
laser "addcon pno1=15 pno2=7"

laser "addcon pno1=7 pno2=10"
laser "addcon pno1=10 pno2=7"

laser "addcon pno1=15 pno2=28"
laser "addcon pno1=28 pno2=15"

laser "addcon pno1=4 pno2=16"
laser "addcon pno1=16 pno2=4"

laser "addcon pno1=5 pno2=17"
laser "addcon pno1=17 pno2=5"

laser "addcon pno1=16 pno2=17"
laser "addcon pno1=17 pno2=16"

laser "addcon pno1=16 pno2=29"
laser "addcon pno1=29 pno2=16"

laser "addcon pno1=8 pno2=16"
laser "addcon pno1=16 pno2=8"

laser "addcon pno1=9 pno2=18"
laser "addcon pno1=18 pno2=9"

laser "addcon pno1=16 pno2=18"
laser "addcon pno1=18 pno2=16"

laser "addcon pno1=8 pno2=11"
laser "addcon pno1=11 pno2=8"

laser "addcon pno1=19 pno2=20"
laser "addcon pno1=20 pno2=19"

laser "addcon pno1=10 pno2=11"
laser "addcon pno1=11 pno2=10"

laser "addcon pno1=20 pno2=12"
laser "addcon pno1=12 pno2=20"

laser "addcon pno1=20 pno2=13"
laser "addcon pno1=13 pno2=20"

laser "addcon pno1=12 pno2=13"
laser "addcon pno1=13 pno2=12"

laser "addcon pno1=12 pno2=22"
laser "addcon pno1=22 pno2=12"

laser "addcon pno1=22 pno2=24"
laser "addcon pno1=24 pno2=22"

laser "addcon pno1=13 pno2=23"
laser "addcon pno1=23 pno2=13"

laser "addcon pno1=23 pno2=25"
laser "addcon pno1=25 pno2=23"

laser "addcon pno1=19 pno2=26"
laser "addcon pno1=26 pno2=19"

laser "addcon pno1=19 pno2=27"
laser "addcon pno1=27 pno2=19"

laser "addcon pno1=27 pno2=26"
laser "addcon pno1=26 pno2=27"

laser "addcon pno1=28 pno2=26"
laser "addcon pno1=26 pno2=28"

laser "addcon pno1=27 pno2=29"
laser "addcon pno1=29 pno2=27"

laser "addcon pno1=1 pno2=30"
laser "addcon pno1=30 pno2=1"

laser "addcon pno1=30 pno2=31"
laser "addcon pno1=31 pno2=30"

%----------------------------------------------
%cost calculation
laser "calculatecost"
%----------------------------------------------
%add walls

laser "addline startx=0 starty=0 endx=0 endy=1.8 name='WD'"
laser "addline startx=0 starty=0 endx=1.8 endy=0 name='SL'"
laser "addline startx=4 starty=0 endx=2.2 endy=0 name='SR'"
laser "addline startx=4 starty=0 endx=4 endy=1.8 name='ED'"
laser "addline startx=0 starty=5 endx=0 endy=3.2 name='WU'"
laser "addline startx=0 starty=5 endx=1.8 endy=5 name='NL'"
laser "addline startx=4 starty=5 endx=2.2 endy=5 name='NR'"
laser "addline startx=4 starty=5 endx=4 endy=3.2 name='EU'"
laser "addline startx=1.7 starty=2.5 endx=1.7 endy=3.1 name='M1'"
laser "addline startx=1.7 starty=3.1 endx=0.9 endy=3.1 name='M2'"
laser "addline startx=0.9 starty=3.1 endx=0.9 endy=4.3 name='M3'"
laser "addline startx=0.9 starty=4.3 endx=3.1 endy=4.3 name='M4'"
laser "addline startx=2 starty=3.7 endx=2 endy=4.3 name='M5'"
laser "addline startx=1.5 starty=3.7 endx=2.5 endy=3.7 name='M6'"
laser "addline startx=3.1 starty=4.3 endx=3.1 endy=3.1 name='M7'"
laser "addline startx=3.1 starty=3.1 endx=2.3 endy=3.1 name='M8'"
laser "addline startx=2.3 starty=3.1 endx=2.3 endy=2.5 name='M9'"
laser "addline startx=4.2 starty=2.4 endx=4.2 endy=2.6 name='T1'"
laser "addline startx=4.2 starty=2.6 endx=4.3 endy=2.6 name='T2'"
laser "addline startx=4.3 starty=2.6 endx=4.3 endy=2.4 name='T3'"
laser "addline startx=4.3 starty=2.4 endx=4.2 endy=2.4 name='T4'"

%----------------------------------------------------------------
%set initial position
laser "setinitpose x=0.5 y=0.5 th=0"
laser "setinitcov Cx=0.1 Cy=0.1 Cth=0.1"
set "$odox" 0.5
set "$odoy" 0.5
set "$odoth" 0

laser "scanset def=urg"
laser "scanset logopen"
laser "scanset log=1"
laser "odopose log=true"
%----------------------------------------------------------------
%set localizer period
laser "push t='0.5' cmd='localize'"
%laser "localize"

%----------------------------------------------------------------
%guidemark reading point coordinates and angles
array "gmx" 23
array "gmy" 23
array "gmth" 23
pi=3.1416
gmx[1]=-0.5
gmx[2]=0.5
gmx[3]=3.5
gmx[4]=4.5
gmx[5]=-0.5
gmx[6]=0.5
gmx[7]=3.5
gmx[8]=4.5
gmx[9]=0.5
gmx[10]=3.5
gmx[11]=0.5
gmx[12]=3.5
gmx[13]=1.3
gmx[14]=2.9
gmx[15]=3.5
gmx[16]=3.5
gmx[17]=2.5
gmx[18]=1.5
gmx[19]=0.5
gmx[20]=1.5
gmx[21]=2.5
gmx[22]=0.5

gmy[1]=1.5
gmy[2]=1.5
gmy[3]=1.5
gmy[4]=1.5
gmy[5]=3.5
gmy[6]=3.5
gmy[7]=3.5
gmy[8]=3.5
gmy[9]=4.6
gmy[10]=4.6
gmy[11]=4.6
gmy[12]=4.6
gmy[13]=4
gmy[14]=4
gmy[15]=1.5
gmy[16]=1.5
gmy[17]=2.2
gmy[18]=2.2
gmy[19]=1.5
gmy[20]=0.5
gmy[21]=0.65
gmy[22]=0.5


gmth[1]=0
gmth[2]=pi
gmth[3]=0
gmth[4]=pi
gmth[5]=0
gmth[6]=pi
gmth[7]=0
gmth[8]=pi
gmth[9]=pi
gmth[10]=0
gmth[11]=pi/2
gmth[12]=pi/2
gmth[13]=0
gmth[14]=pi
gmth[15]=-pi/2-pi/12
gmth[16]=pi
gmth[17]=-pi/2
gmth[18]=-pi/2
gmth[19]=0
gmth[20]=pi/2
gmth[21]=pi/2
gmth[22]=0


%----------------------------------------------
%send to predefined guidemark
gm=2
%----------------------------------------------
laser "localize getonly"
xp=$odox
yp=$odoy
thp=$odoth
eval xp
eval yp
eval thp
k=65
gmnotfound=0

label "main"
call "findroute"
call "gmark"
if (gmnotfound==1 & gm<15) "failgmk"
if (gm!=99) "main"
label "failgmk"
gm=15
speak "Going to count the shiny round thingies"
call "findroute"

%---------------------
%ballfinding algorithm
%---------------------
vision "imageget"
wait 1
vision "imageget"
wait 1
vision "ball red smrcl"
wait 1
eval $vis4
tointstring $vis4
stringcat "I see " "$str" " red balls"
speak "$string"
vision "ball blue smrcl"
wait 1
eval $vis4
tointstring $vis4
stringcat "and " "$str" " blue balls"
speak "$string"
wait 1

%---------------------
%box finding algorithm
%---------------------
done=15
speak "Searching for tricky square object"
label "findbox"
done=done+1
gm=done
if (gm==22) "finish"
call "findroute"
call "gmark"
if ($gmkx > 2.3 | $guidemarkok<1) "findbox"
invtrans $l0 $l1 $l2 $odox $odoy $odoth
wait 0.1
rx=$res0
ry=$res1
rth=$res2
invtrans $gmkx $gmky $gmkkappa rx ry rth
wait 0.1
angdeg=$res2*180/pi
angdeg=normalizeangledeg(angdeg)
stringcat "Found box at coordinates x equals " $res0 " meters, y equals " $res1 " meters"
speak "$string"
wait 2
goto "finish"

%------------------------------------------------
%route finding
%------------------------------------------------
label "findroute"

targetx=gmx[gm]
targety=gmy[gm]
targetth=gmth[gm]

wait 1
stringcat "findroute startx="xp" starty="yp" endx="targetx" endy="targety" id="k""
laser "$string"
wait 1 :($l9 == k)
eval $l4
Nmax=$l4
k=k+1
%Start sequence
nxtp=Nmax
if (Nmax == 1) "skip"
nxtp = Nmax-1
label "skip"
stringcat "getpoint p=" nxtp " id=" nxtp""
laser "$string"
wait 1:($l9 == nxtp)
angle = atan2(y1,x1)-$odoth+$l2
angle = normalizeanglerad(angle)
ignoreobstacles
turn angle "rad" @v0.2
idle

label "Drive"
stringcat "getpoint p=" Nmax " id=" Nmax""
laser "$string"
wait 1:($l9 == Nmax)
eval $l5
eval $l6
eval $l7
ignoreobstacles
drivew $l5 $l6 $l7 "rad" @v 0.3 :($targetdist < 0.05)
stop
Nmax = Nmax-1
if (Nmax > -1) "Drive"
xp = targetx
yp = targety
%End sequence
laser "localize getonly"
eval $l2
eval $odoth
angle=targetth-$odoth+$l2
angle=normalizeanglerad(angle)
ignoreobstacles
turn angle "rad" @v 0.1
idle
return

%------------------------------------------------
label "gmark"
%------------------------------------------------
%Try reading guidemark
call "tryreading"
if (code > 0) "goodreading"
turn 10 @v0.1
idle
wait 0.2
call "tryreading"
if (code > 0) "goodreading"
turn -20 @v0.1
idle
wait 0.2
call "tryreading"
if (code > 0) "goodreading"
if (gm>14 | gm==99) "bad"
speak "I fail"
goto "bad"

label "goodreading"
if ($gmkx > 2.3) "jmp"
tointstring code
stringcat "Guidemark number  " "$str"
speak "$string"
label "jmp"
wait 0.2
gm=code
return
label "bad"
gmnotfound=1
return

%-----------------------------------------------
%function for reading guidemark
%-----------------------------------------------
label "tryreading"
n=0
label "again"
get "guidemark"
wait 2 : ($guidemarkok)
eval $guidemarkok
code=$guidemarkok
n=n+1
if (n<2) "again"
return

%-----------------------------------------
label "finish"
if (gm!=22) "boxfound"
speak "Could not find box. I am a sad robot"
label "boxfound"
gm=22
speak "I am going home"
call "findroute"
turn 45 @v0.1
fwd -0.2
turn -45 @v0.1
speak "bazinga"
laser "scanset logclose"
laser "odopose log=false"
stop