Friday, May 6, 2011
Thursday, May 5, 2011
Final Project
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
Tuesday, March 29, 2011
Exercise 11, Problem 4
Cleaning the box.
Make a program that 'cleans' the box by going from side to side and back in a pattern that covers the hold box. Aulocalize should be used to assure correct position in the box.
Make a program that 'cleans' the box by going from side to side and back in a pattern that covers the hold box. Aulocalize should be used to assure correct position in the box.
N=10
laser "addline startx=-0.9 starty=0.9 endx=0.9 endy=0.9 name='N'"
laser "addline startx=-0.9 starty=-0.9 endx=0.9 endy=-0.9 name='S'"
laser "addline startx=-0.9 starty=-0.9 endx=-0.9 endy=0.9 name='V'"
laser "addline startx=0.9 starty=-0.9 endx=0.9 endy=0.9 name='E'"
laser "setinitpose x=0 y=0 th=0"
laser "setinitcov Cx=0.1 Cy=0.1 Cth=0.1"
laser "push t='1.0' cmd='localize'"
turn 45
fwd 0.7
turn 135
drivew -0.5 0.5 -180:($targetdist<0.05)
drivew 0.5 0.45 0:($targetdist<0.05)
drivew -0.5 0.4 -180:($targetdist<0.05)
drivew 0.5 0.35 0:($targetdist<0.05)
drivew -0.5 0.3 -180:($targetdist<0.05)
drivew 0.5 0.25 0:($targetdist<0.05)
drivew -0.5 0.2 -180:($targetdist<0.05)
drivew 0.5 0.15 0:($targetdist<0.05)
drivew -0.5 0.1 -180:($targetdist<0.05)
drivew 0.5 0.05 0:($targetdist<0.05)
drivew -0.5 0.0 -180:($targetdist<0.05)
drivew 0.5 -0.05 0:($targetdist<0.05)
drivew -0.5 -0.1 -180:($targetdist<0.05)
drivew 0.5 -0.15 0:($targetdist<0.05)
drivew -0.5 -0.2 -180:($targetdist<0.05)
drivew 0.5 -0.25 0:($targetdist<0.05)
drivew -0.5 -0.3 -180:($targetdist<0.05)
drivew 0.5 -0.35 0:($targetdist<0.05)
drivew -0.5 -0.4 -180:($targetdist<0.05)
drivew 0.5 -0.45 0:($targetdist<0.05)
drivew -0.5 -0.5 -180:($targetdist<0.05)
drivew 0.5 0.5 90:($targetdist<0.05)
laser "addline startx=-0.9 starty=0.9 endx=0.9 endy=0.9 name='N'"
laser "addline startx=-0.9 starty=-0.9 endx=0.9 endy=-0.9 name='S'"
laser "addline startx=-0.9 starty=-0.9 endx=-0.9 endy=0.9 name='V'"
laser "addline startx=0.9 starty=-0.9 endx=0.9 endy=0.9 name='E'"
laser "setinitpose x=0 y=0 th=0"
laser "setinitcov Cx=0.1 Cy=0.1 Cth=0.1"
laser "push t='1.0' cmd='localize'"
turn 45
fwd 0.7
turn 135
drivew -0.5 0.5 -180:($targetdist<0.05)
drivew 0.5 0.45 0:($targetdist<0.05)
drivew -0.5 0.4 -180:($targetdist<0.05)
drivew 0.5 0.35 0:($targetdist<0.05)
drivew -0.5 0.3 -180:($targetdist<0.05)
drivew 0.5 0.25 0:($targetdist<0.05)
drivew -0.5 0.2 -180:($targetdist<0.05)
drivew 0.5 0.15 0:($targetdist<0.05)
drivew -0.5 0.1 -180:($targetdist<0.05)
drivew 0.5 0.05 0:($targetdist<0.05)
drivew -0.5 0.0 -180:($targetdist<0.05)
drivew 0.5 -0.05 0:($targetdist<0.05)
drivew -0.5 -0.1 -180:($targetdist<0.05)
drivew 0.5 -0.15 0:($targetdist<0.05)
drivew -0.5 -0.2 -180:($targetdist<0.05)
drivew 0.5 -0.25 0:($targetdist<0.05)
drivew -0.5 -0.3 -180:($targetdist<0.05)
drivew 0.5 -0.35 0:($targetdist<0.05)
drivew -0.5 -0.4 -180:($targetdist<0.05)
drivew 0.5 -0.45 0:($targetdist<0.05)
drivew -0.5 -0.5 -180:($targetdist<0.05)
drivew 0.5 0.5 90:($targetdist<0.05)
Exercise 11, Problem 3
Boxdriving with Kalman Localization.
Change the code from problem two so it uses kalman updates from the localizer plugin.
Change the code from problem two so it uses kalman updates from the localizer plugin.
periodic Kalman filtering
N=10
laser "addline startx=-0.9 starty=0.9 endx=0.9 endy=0.9 name='N'"
laser "addline startx=-0.9 starty=-0.9 endx=0.9 endy=-0.9 name='S'"
laser "addline startx=-0.9 starty=-0.9 endx=-0.9 endy=0.9 name='V'"
laser "addline startx=0.9 starty=-0.9 endx=0.9 endy=0.9 name='E'"
laser "setinitpose x=0 y=0 th=0"
laser "setinitcov Cx=0.1 Cy=0.1 Cth=0.1"
laser "push t='1.0' cmd='localize'"
label "run"
stop
ignoreobstacles
drivew 0.4 0.4 90:($targetdist<0.05)
stop
ignoreobstacles
drivew -0.4 0.4 -180:($targetdist<0.05)
stop
ignoreobstacles
drivew -0.4 -0.4 -90:($targetdist<0.05)
stop
ignoreobstacles
drivew 0.4 -0.4 0:($targetdist<0.05)
N=N-1
if(N>0) "run"
each corner Kalman filtering
N=10
laser "addline startx=-0.9 starty=0.9 endx=0.9 endy=0.9 name='N'"
laser "addline startx=-0.9 starty=-0.9 endx=0.9 endy=-0.9 name='S'"
laser "addline startx=-0.9 starty=-0.9 endx=-0.9 endy=0.9 name='V'"
laser "addline startx=0.9 starty=-0.9 endx=0.9 endy=0.9 name='E'"
laser "setinitpose x=0 y=0 th=0"
laser "setinitcov Cx=0.1 Cy=0.1 Cth=0.1"
laser "localize"
label "run"
stop
wait 0.5
laser "localize"
ignoreobstacles
drivew 0.5 0.5 90:($targetdist<0.05)
stop
wait 0.5
laser "localize"
ignoreobstacles
drivew -0.5 0.5 -180:($targetdist<0.05)
stop
wait 0.5
laser "localize"
ignoreobstacles
drivew -0.5 -0.5 -90:($targetdist<0.05)
stop
wait 0.5
laser "localize"
ignoreobstacles
drivew 0.5 -0.5 0:($targetdist<0.05)
N=N-1
if(N>0) "run"
N=10
laser "addline startx=-0.9 starty=0.9 endx=0.9 endy=0.9 name='N'"
laser "addline startx=-0.9 starty=-0.9 endx=0.9 endy=-0.9 name='S'"
laser "addline startx=-0.9 starty=-0.9 endx=-0.9 endy=0.9 name='V'"
laser "addline startx=0.9 starty=-0.9 endx=0.9 endy=0.9 name='E'"
laser "setinitpose x=0 y=0 th=0"
laser "setinitcov Cx=0.1 Cy=0.1 Cth=0.1"
laser "push t='1.0' cmd='localize'"
label "run"
stop
ignoreobstacles
drivew 0.4 0.4 90:($targetdist<0.05)
stop
ignoreobstacles
drivew -0.4 0.4 -180:($targetdist<0.05)
stop
ignoreobstacles
drivew -0.4 -0.4 -90:($targetdist<0.05)
stop
ignoreobstacles
drivew 0.4 -0.4 0:($targetdist<0.05)
N=N-1
if(N>0) "run"
each corner Kalman filtering
N=10
laser "addline startx=-0.9 starty=0.9 endx=0.9 endy=0.9 name='N'"
laser "addline startx=-0.9 starty=-0.9 endx=0.9 endy=-0.9 name='S'"
laser "addline startx=-0.9 starty=-0.9 endx=-0.9 endy=0.9 name='V'"
laser "addline startx=0.9 starty=-0.9 endx=0.9 endy=0.9 name='E'"
laser "setinitpose x=0 y=0 th=0"
laser "setinitcov Cx=0.1 Cy=0.1 Cth=0.1"
laser "localize"
label "run"
stop
wait 0.5
laser "localize"
ignoreobstacles
drivew 0.5 0.5 90:($targetdist<0.05)
stop
wait 0.5
laser "localize"
ignoreobstacles
drivew -0.5 0.5 -180:($targetdist<0.05)
stop
wait 0.5
laser "localize"
ignoreobstacles
drivew -0.5 -0.5 -90:($targetdist<0.05)
stop
wait 0.5
laser "localize"
ignoreobstacles
drivew 0.5 -0.5 0:($targetdist<0.05)
N=N-1
if(N>0) "run"
Exercise 11, Problem 2
Generation of Boxdriving in SMRCL
Make smrcl code that makes an smr follow the targetposes starting in the middle of the box then going to the corners and then continueing N times around in the square defined by the cornerpoints.
Make smrcl code that makes an smr follow the targetposes starting in the middle of the box then going to the corners and then continueing N times around in the square defined by the cornerpoints.
laser "addline startx=-0.9 starty=0.9 endx=0.9 endy=0.9 name='N'"
laser "addline startx=-0.9 starty=-0.9 endx=0.9 endy=-0.9 name='S'"
laser "addline startx=-0.9 starty=-0.9 endx=-0.9 endy=0.9 name='V'"
laser "addline startx=0.9 starty=-0.9 endx=0.9 endy=0.9 name='E'"
laser "setinitpose x=0 y=0 th=0"
laser "setinitcov Cx=0.1 Cy=0.1 Cth=0.1"
laser "localize"
ignoreobstacles
drive 0.4 0.4 90:($targetdist<0.05)
ignoreobstacles
drive -0.4 0.4 -180:($targetdist<0.05)
ignoreobstacles
drive -0.4 -0.4 -90:($targetdist<0.05)
ignoreobstacles
drive 0.4 -0.4 0:($targetdist<0.05)
ignoreobstacles
drive 0.4 0.4 90:($targetdist<0.05)
laser "addline startx=-0.9 starty=-0.9 endx=0.9 endy=-0.9 name='S'"
laser "addline startx=-0.9 starty=-0.9 endx=-0.9 endy=0.9 name='V'"
laser "addline startx=0.9 starty=-0.9 endx=0.9 endy=0.9 name='E'"
laser "setinitpose x=0 y=0 th=0"
laser "setinitcov Cx=0.1 Cy=0.1 Cth=0.1"
laser "localize"
ignoreobstacles
drive 0.4 0.4 90:($targetdist<0.05)
ignoreobstacles
drive -0.4 0.4 -180:($targetdist<0.05)
ignoreobstacles
drive -0.4 -0.4 -90:($targetdist<0.05)
ignoreobstacles
drive 0.4 -0.4 0:($targetdist<0.05)
ignoreobstacles
drive 0.4 0.4 90:($targetdist<0.05)
Exercise 11, Problem 1
Initialization of the localiser.
The localizer is know ready. It will make a new pose estimate each time the command 'localize' is given.
laser cmd="addline startx=-0.9 starty=0.9 endx=0.9 endy=0.9 name='N'"
laser cmd="addline startx=-0.9 starty=-0.9 endx=0.9 endy=-0.9 name='S'"
laser cmd="addline startx=-0.9 starty=-0.9 endx=-0.9 endy=0.9 name='V'"
laser cmd="addline startx=0.9 starty=-0.9 endx=0.9 endy=0.9 name='E'"
laser cmd="setinitpose x=0 y=0 th=0"
laser cmd="setinitcov Cx=0.1 Cy=0.1 Cth=0.1"
laser cmd="localize"
laser cmd="addline startx=-0.9 starty=-0.9 endx=0.9 endy=-0.9 name='S'"
laser cmd="addline startx=-0.9 starty=-0.9 endx=-0.9 endy=0.9 name='V'"
laser cmd="addline startx=0.9 starty=-0.9 endx=0.9 endy=0.9 name='E'"
laser cmd="setinitpose x=0 y=0 th=0"
laser cmd="setinitcov Cx=0.1 Cy=0.1 Cth=0.1"
laser cmd="localize"
Monday, March 28, 2011
Exercise 8, Problem 2.2
After fervently fighting for hours with correct matrix sizing, we can see that the system is now more accurate.
Exercise 12, Task 6
The function below creates a file that contains instructions for the robot depending on the resulted list of cells.
function SMRfile(q)
fid = fopen('SMRcommand.txt','w');
for i=1:(size(q,1)-1)
x= 0.05*(q(i+1,1)-q(i,1));
y= 0.05*(q(i+1,2)-q(i,2));
if(y==0 && x>0)
th=0;
elseif(y==0 && x<0)
th=180;
elseif(x==0 && y>0)
th=90;
elseif(x==0 && y<0)
th=-90;
elseif(x>0 && y>0)
th=45;
elseif(x>0 && y<0)
th=-45;
elseif(y>0)
th=135;
else
th=-135;
end
fprintf(fid,'drive %f %f %f:($targetdist<0)\n',x,y,th);
end
fprintf(fid,'stop');
fclose(fid)
end
function SMRfile(q)
fid = fopen('SMRcommand.txt','w');
for i=1:(size(q,1)-1)
x= 0.05*(q(i+1,1)-q(i,1));
y= 0.05*(q(i+1,2)-q(i,2));
if(y==0 && x>0)
th=0;
elseif(y==0 && x<0)
th=180;
elseif(x==0 && y>0)
th=90;
elseif(x==0 && y<0)
th=-90;
elseif(x>0 && y>0)
th=45;
elseif(x>0 && y<0)
th=-45;
elseif(y>0)
th=135;
else
th=-135;
end
fprintf(fid,'drive %f %f %f:($targetdist<0)\n',x,y,th);
end
fprintf(fid,'stop');
fclose(fid)
end
Exercise 12, Task 5
The results for the given case are:
distmap =
3.41 3.00 0 0 0 0 0 0 0 0
3.00 2.00 0 14.82 15.23 15.64 16.05 16.46 16.87 0
0 3.00 0 13.82 14.23 14.64 15.05 15.46 16.46 0
0 4.00 0 12.82 13.23 13.64 14.05 15.05 16.05 0
0 5.00 0 11.82 12.23 12.64 13.64 14.64 15.64 0
0 6.00 0 10.82 11.23 12.23 13.23 14.23 15.23 0
0 7.00 0 9.82 10.82 11.82 12.82 13.82 14.82 0
0 8.00 8.41 9.41 10.41 11.41 12.41 13.41 14.41 0
0 9.00 9.41 9.82 10.82 11.82 12.82 13.82 14.82 0
0 10.00 10.41 10.82 11.23 12.23 13.23 14.23 0 0
route =
9 9
8 8
8 7
8 6
8 5
8 4
8 3
7 2
6 2
5 2
4 2
3 2
2 2
map =
0 0 1 0 0 0 0 0 0 0
0 2 1 0 0 0 0 0 0 0
1 2 1 0 0 0 0 0 0 0
1 2 1 0 0 0 0 0 0 0
1 2 1 0 0 0 0 0 0 0
1 2 1 0 0 0 0 0 0 0
1 2 1 0 0 0 0 0 0 0
1 0 2 2 2 2 2 2 0 0
1 0 0 0 0 0 0 0 2 0
1 0 0 0 0 0 0 0 0 0
distmap =
3.41 3.00 0 0 0 0 0 0 0 0
3.00 2.00 0 14.82 15.23 15.64 16.05 16.46 16.87 0
0 3.00 0 13.82 14.23 14.64 15.05 15.46 16.46 0
0 4.00 0 12.82 13.23 13.64 14.05 15.05 16.05 0
0 5.00 0 11.82 12.23 12.64 13.64 14.64 15.64 0
0 6.00 0 10.82 11.23 12.23 13.23 14.23 15.23 0
0 7.00 0 9.82 10.82 11.82 12.82 13.82 14.82 0
0 8.00 8.41 9.41 10.41 11.41 12.41 13.41 14.41 0
0 9.00 9.41 9.82 10.82 11.82 12.82 13.82 14.82 0
0 10.00 10.41 10.82 11.23 12.23 13.23 14.23 0 0
route =
9 9
8 8
8 7
8 6
8 5
8 4
8 3
7 2
6 2
5 2
4 2
3 2
2 2
map =
0 0 1 0 0 0 0 0 0 0
0 2 1 0 0 0 0 0 0 0
1 2 1 0 0 0 0 0 0 0
1 2 1 0 0 0 0 0 0 0
1 2 1 0 0 0 0 0 0 0
1 2 1 0 0 0 0 0 0 0
1 2 1 0 0 0 0 0 0 0
1 0 2 2 2 2 2 2 0 0
1 0 0 0 0 0 0 0 2 0
1 0 0 0 0 0 0 0 0 0
Exercise 12, Task 4
After computing the distance map with the previous function, the following one finds the minimum distance route from the start point to the final one.
function route = findroute(startcell,finalcell,distmap)
global map;
q = startcell;
cell = startcell;
d = 100000;
while(cell(1)~=finalcell(1) || cell(2)~=finalcell(2))
n = neighbours(cell);
for i=1:size(n,1)
if(map(n(i,1),n(i,2))==0 && distmap(n(i,1),n(i,2))~=0 && distmap(n(i,1),n(i,2))<d)
d = distmap(n(i,1),n(i,2));
next = n(i,:)
end
end
q = insert(next,q);
cell = next;
end
for i=1:size(q,1)
map(q(i,1),q(i,2))=2;
end
map
route = q
end
The function returns the list of cells that form the route as well as the map which shows the route (represented by values of 2)
function route = findroute(startcell,finalcell,distmap)
global map;
q = startcell;
cell = startcell;
d = 100000;
while(cell(1)~=finalcell(1) || cell(2)~=finalcell(2))
n = neighbours(cell);
for i=1:size(n,1)
if(map(n(i,1),n(i,2))==0 && distmap(n(i,1),n(i,2))~=0 && distmap(n(i,1),n(i,2))<d)
d = distmap(n(i,1),n(i,2));
next = n(i,:)
end
end
q = insert(next,q);
cell = next;
end
for i=1:size(q,1)
map(q(i,1),q(i,2))=2;
end
map
route = q
end
The function returns the list of cells that form the route as well as the map which shows the route (represented by values of 2)
Exercise 12, Task 3
The function starts in the final point and calculates the minimum distances of the cells until it reaches the start point.
function matr = makewave(startcell,finalcell)
global map;
distmap = zeros(size(map,1),size(map,2));
distmap(finalcell(1),finalcell(2))=2;
queue = finalcell;
reached = 0;
while(size(queue,1) && reached==0)
[crt,queue] = retrieve(queue);
n = neighbours(crt);
for i=1:size(n,1)
if(map(n(i,1),n(i,2))==0)
d = distmap(crt(1),crt(2)) + celldist(crt,n(i,:));
if(distmap(n(i,1),n(i,2))==0)
distmap(n(i,1),n(i,2))= d;
queue = insert(n(i,:),queue);
elseif(distmap(n(i,1),n(i,2)) > d)
distmap(n(i,1),n(i,2)) = d;
end
end
if(n(i,:)==startcell)
reached = 1;
end
end
end
if(reached==0)
fprintf('The wave did not reach the starting point.')
end
matr = distmap;
end
Exercise 12, Task 2
The function to find the neighbours of a cell is:
function list = neighbours(cell)
global map;
x = cell(1);
y = cell(2);
if(x==1 && y==1)
list = [ x y+1;
x+1 y+1;
x+1 y];
elseif(x==1 && y==size(map,2))
list = [x+1 y;
x y-1;
x+1 y-1];
elseif(x==1)
list = [ x y+1;
x+1 y+1;
x+1 y;
x y-1;
x+1 y-1];
elseif(y==1 && x==size(map,1))
list = [x-1 y+1;
x y+1;
x-1 y];
elseif(y==1)
list = [x-1 y+1;
x y+1;
x+1 y+1;
x-1 y;
x+1 y];
elseif(x==size(map,1) && y==size(map,2))
list = [x-1 y;
x-1 y-1;
x y-1];
elseif(x==size(map,1))
list = [x-1 y+1;
x y+1;
x-1 y;
x-1 y-1;
x y-1];
elseif(y==size(map,2))
list = [x-1 y;
x+1 y;
x-1 y-1;
x y-1;
x+1 y-1];
else
list = [x-1 y+1;
x y+1;
x+1 y+1;
x-1 y;
x+1 y;
x-1 y-1;
x y-1;
x+1 y-1];
end
end
Note: Only the valid neighbours of the cell are taken, i.e. the ones that do not exceed the borders of the matrix.
function list = neighbours(cell)
global map;
x = cell(1);
y = cell(2);
if(x==1 && y==1)
list = [ x y+1;
x+1 y+1;
x+1 y];
elseif(x==1 && y==size(map,2))
list = [x+1 y;
x y-1;
x+1 y-1];
elseif(x==1)
list = [ x y+1;
x+1 y+1;
x+1 y;
x y-1;
x+1 y-1];
elseif(y==1 && x==size(map,1))
list = [x-1 y+1;
x y+1;
x-1 y];
elseif(y==1)
list = [x-1 y+1;
x y+1;
x+1 y+1;
x-1 y;
x+1 y];
elseif(x==size(map,1) && y==size(map,2))
list = [x-1 y;
x-1 y-1;
x y-1];
elseif(x==size(map,1))
list = [x-1 y+1;
x y+1;
x-1 y;
x-1 y-1;
x y-1];
elseif(y==size(map,2))
list = [x-1 y;
x+1 y;
x-1 y-1;
x y-1;
x+1 y-1];
else
list = [x-1 y+1;
x y+1;
x+1 y+1;
x-1 y;
x+1 y;
x-1 y-1;
x y-1;
x+1 y-1];
end
end
Note: Only the valid neighbours of the cell are taken, i.e. the ones that do not exceed the borders of the matrix.
Exercise 12, Task 1
To add or extract elements from a queue, the functions below were used:
function q = insert(cell,queue)
i=size(queue,1)+1;
queue(i,:)=cell;
q = queue;
end
function [val,q] = retrieve(queue)
val = queue(1,:);
q = queue(2:(size(queue,1)),:);
end
function q = insert(cell,queue)
i=size(queue,1)+1;
queue(i,:)=cell;
q = queue;
end
function [val,q] = retrieve(queue)
val = queue(1,:);
q = queue(2:(size(queue,1)),:);
end
Exercise 9, Problem 1.2
Nabla H is calculated as stated in the previous exercise, but taking into account the displacement of the laser from the robot. We then obtain:
Exercise 10, Problem 2
Problem 2 Generation of target poses for driving in a box.
The box has 5 points: the 4 corners and the starting point (pose) in mainloop.m file:
The sequence of the points with exception handling in mainloop.m file:
Constants setup in file constants.m:
The box has 5 points: the 4 corners and the starting point (pose) in mainloop.m file:
places = [ [0; 0; 0] [0.5; 0.5; pi] [-0.5; 0.5; -pi/2] [-0.5; -0.5; 0] [0.5; -0.5; pi/2] [0.5; 0.5; pi] ];
The sequence of the points with exception handling in mainloop.m file:
if exist('nextPose','var') == 0 %
nextPose = 1;
elseif nextPose >= length(places(1,:))
nextPose = 1;
else
nextPose = nextPose + 1;
end
nextPose = 1;
elseif nextPose >= length(places(1,:))
nextPose = 1;
else
nextPose = nextPose + 1;
end
Constants setup in file constants.m:
simulation = true;
noOfIter = 16; %The number of simulation iterations
noOfIter = 16; %The number of simulation iterations
Exercise 10, Problem 2.1
Simulate Boxdriving. Run the framework and see that the boxdrive works in simulation.
The result of the simulation reveals the errors in odometry:
The result of the simulation reveals the errors in odometry:
Exercise 10, Problem 1.1
Calculation of targetpose in odometry coordinates.
function out = trans(transform,targetPose)
% out <-> odoTargetPose (notation)
% odoTargetPose = TRANS(transform,targetPose)
% Transform a given point in world coordinates (targetPose) to odometry
% coordinates, using the origo of the odometry coordinates in world
% coordinates (transform).
%calculation of targetpose in ordinary coordinates
t=transform; %for shorter name
tMatrix=[cos(t(3)) -sin(t(3)) 0; sin(t(3)) cos(t(3)) 0; 0 0 1];
temp = tMatrix*targetPose ;
out = temp + t;
out(3) = normalizeAngle(out(3));
end
function out = trans(transform,targetPose)
% out <-> odoTargetPose (notation)
% odoTargetPose = TRANS(transform,targetPose)
% Transform a given point in world coordinates (targetPose) to odometry
% coordinates, using the origo of the odometry coordinates in world
% coordinates (transform).
%calculation of targetpose in ordinary coordinates
t=transform; %for shorter name
tMatrix=[cos(t(3)) -sin(t(3)) 0; sin(t(3)) cos(t(3)) 0; 0 0 1];
temp = tMatrix*targetPose ;
out = temp + t;
out(3) = normalizeAngle(out(3));
end
Exercise 10, Problem 1
Calculation of transformation from world coordinates to odometry coordinates.
function transform = findTransform(odoPose, pose)
% transform = FINDTRANSFORM(odoPose,pose)
% Find the transformation from the world coordinates to the odometry
% coordinates given a pose in the odometry coordinates (odoPose) and the
% same point in the world coordinates (pose). The output (transform) is
% simply the origo of the odometry coordinates in the world coordinates
theta = normalizeAngle(odoPose(3)-pose(3));
tMatrix = [cos(theta) -sin(theta) 0;sin(theta) cos(theta) 0;0 0 1];
transform = -tMatrix*pose + odoPose;
transform(3) = normalizeAngle(transform(3));
end
function outAngle = normalizeAngle(inAngle)
inAngle = inAngle + 2*pi;
outAngle = mod(inAngle,2*pi);
outAngle = outAngle -2*pi;
end
function transform = findTransform(odoPose, pose)
% transform = FINDTRANSFORM(odoPose,pose)
% Find the transformation from the world coordinates to the odometry
% coordinates given a pose in the odometry coordinates (odoPose) and the
% same point in the world coordinates (pose). The output (transform) is
% simply the origo of the odometry coordinates in the world coordinates
theta = normalizeAngle(odoPose(3)-pose(3));
tMatrix = [cos(theta) -sin(theta) 0;sin(theta) cos(theta) 0;0 0 1];
transform = -tMatrix*pose + odoPose;
transform(3) = normalizeAngle(transform(3));
end
Where we have used the angle normalization:
function outAngle = normalizeAngle(inAngle)
inAngle = inAngle + 2*pi;
outAngle = mod(inAngle,2*pi);
outAngle = outAngle -2*pi;
end
Thursday, March 24, 2011
Exercise 6
The script for ball navigation:
pi=3.14159265
ah=0
ad=100
turn 45
wait 1
vision "ball2 debug=true blue=false smrcl"
wait 1
eval $vis1
ad1=1000
if ($vis1<0.5)>
ad1=sqrt($vis3*$vis3+$vis2*$vis2)
x1=$vis2
y1=$vis3
th1=pi/4
label "label11"
turn -45
wait 1
vision "ball2 debug=true blue=false smrcl"
wait 1
eval $vis1
ad2=1000
if ($vis1<0.5)>
ad2=sqrt($vis3*$vis3+$vis2*$vis2)
x2=$vis2
y2=$vis3
th2=0
label "label12"
turn -45
wait 1
vision "ball2 debug=true blue=false smrcl"
wait 1
eval $vis1
ad3=1000
if ($vis1<0.5)>
ad3=sqrt($vis3*$vis3+$vis2*$vis2)
x3=$vis2
y3=$vis3
th3=-pi/4
label "label13"
turn 45
wait 1
if (ad1>ad) "label14"
x=x1
y=y1
th=th1
label "label14"
if (ad2>ad) "label15"
x=x2
y=y2
th=th2
label "label15"
if (ad3>ad) "label16"
x=x3
y=y3
th=th3
label "label16"
ahc=atan2(y,x)
ahc=ahc+th
eval ahc
if (ahc>0) "label17"
y=y+0.5
ah=atan2(y,x)
eval th
ah=ah+th
ah=180*(ah/pi)
eval ah
ad=0.7*(sqrt(y*y+x*x))
turn ah
eval ad
fwd ad@v0.5
aht=-ah
turn aht
goto "label18"
label "label17"
x=x-0.5
ah=atan2(y,x)
eval th
ah=ah+th
ah=180*(ah/pi)
eval ah
ad=sqrt(y*y+x*x)
turn ah
eval ad
fwd ad@v0.5
aht=90-ah
turn aht
turnr 0.5 -90
label "label18"
fwd 0.5
ah=0
ad=100
turn 45
wait 1
vision "ball2 debug=true blue=false smrcl"
wait 1
eval $vis1
ad1=1000
if ($vis1<0.5)>
ad1=sqrt($vis3*$vis3+$vis2*$vis2)
x1=$vis2
y1=$vis3
th1=pi/4
label "label19"
turn -45
wait 1
vision "ball2 debug=true blue=false smrcl"
wait 1
eval $vis1
ad2=1000
if ($vis1<0.5)>
ad2=sqrt($vis3*$vis3+$vis2*$vis2)
x2=$vis2
y2=$vis3
th2=0
label "label110"
turn -45
wait 1
vision "ball2 debug=true blue=false smrcl"
wait 1
eval $vis1
ad3=1000
if ($vis1<0.5)>
ad3=sqrt($vis3*$vis3+$vis2*$vis2)
x3=$vis2
y3=$vis3
th3=-pi/4
label "label111"
turn 45
wait 1
if (ad1>ad) "label112"
x=x1
y=y1
th=th1
label "label112"
if (ad2>ad) "label113"
x=x2
y=y2
th=th2
label "label113"
if (ad3>ad) "label114"
x=x3
y=y3
th=th3
label "label114"
ahc=atan2(y,x)
ahc=ahc+th
eval ahc
if (ahc>0) "label115"
y=y+0.5
ah=atan2(y,x)
eval th
ah=ah+th
ah=180*(ah/pi)
eval ah
ad=0.7*(sqrt(y*y+x*x))
turn ah
eval ad
fwd ad@v0.5
aht=-ah
turn aht
goto "label116"
label "label115"
x=x-0.5
ah=atan2(y,x)
eval th
ah=ah+th
ah=180*(ah/pi)
eval ah
ad=sqrt(y*y+x*x)
turn ah
eval ad
fwd ad@v0.5
aht=90-ah
turn aht
turnr 0.5 -90
label "label116"
fwd 0.5
ah=0
ad=100
turn 45
wait 1
vision "ball2 debug=true blue=false smrcl"
wait 1
eval $vis1
ad1=1000
if ($vis1<0.5)>
ad1=sqrt($vis3*$vis3+$vis2*$vis2)
x1=$vis2
y1=$vis3
th1=pi/4
label "label117"
turn -45
wait 1
vision "ball2 debug=true blue=false smrcl"
wait 1
eval $vis1
ad2=1000
if ($vis1<0.5)>
ad2=sqrt($vis3*$vis3+$vis2*$vis2)
x2=$vis2
y2=$vis3
th2=0
label "label118"
turn -45
wait 1
vision "ball2 debug=true blue=false smrcl"
wait 1
eval $vis1
ad3=1000
if ($vis1<0.5)>
ad3=sqrt($vis3*$vis3+$vis2*$vis2)
x3=$vis2
y3=$vis3
th3=-pi/4
label "label119"
turn 45
wait 1
if (ad1>ad) "label120"
x=x1
y=y1
th=th1
label "label120"
if (ad2>ad) "label121"
x=x2
y=y2
th=th2
label "label121"
if (ad3>ad) "label122"
x=x3
y=y3
th=th3
label "label122"
ahc=atan2(y,x)
ahc=ahc+th
eval ahc
if (ahc>0) "label123"
y=y+0.5
ah=atan2(y,x)
eval th
ah=ah+th
ah=180*(ah/pi)
eval ah
ad=0.7*(sqrt(y*y+x*x))
turn ah
eval ad
fwd ad@v0.5
aht=-ah
turn aht
goto "label124"
label "label123"
x=x-0.5
ah=atan2(y,x)
eval th
ah=ah+th
ah=180*(ah/pi)
eval ah
ad=sqrt(y*y+x*x)
turn ah
eval ad
fwd ad@v0.5
aht=90-ah
turn aht
turnr 0.5 -90
label "label124"
fwd 1.5
turn 180
ah=0
ad=100
turn 45
wait 1
vision "ball2 debug=true blue=false smrcl"
wait 1
eval $vis1
ad1=1000
if ($vis1<0.5)>
ad1=sqrt($vis3*$vis3+$vis2*$vis2)
x1=$vis2
y1=$vis3
th1=pi/4
label "label21"
turn -45
wait 1
vision "ball2 debug=true blue=false smrcl"
wait 1
eval $vis1
ad2=1000
if ($vis1<0.5)>
ad2=sqrt($vis3*$vis3+$vis2*$vis2)
x2=$vis2
y2=$vis3
th2=0
label "label22"
turn -45
wait 1
vision "ball2 debug=true blue=false smrcl"
wait 1
eval $vis1
ad3=1000
if ($vis1<0.5)>
ad3=sqrt($vis3*$vis3+$vis2*$vis2)
x3=$vis2
y3=$vis3
th3=-pi/4
label "label23"
turn 45
wait 1
if (ad1>ad) "label24"
x=x1
y=y1
th=th1
label "label24"
if (ad2>ad) "label25"
x=x2
y=y2
th=th2
label "label25"
if (ad3>ad) "label26"
x=x3
y=y3
th=th3
label "label26"
ahc=atan2(y,x)
ahc=ahc+th
eval ahc
if (ahc>0) "label27"
y=y+0.5
ah=atan2(y,x)
eval th
ah=ah+th
ah=180*(ah/pi)
eval ah
ad=0.7*(sqrt(y*y+x*x))
turn ah
eval ad
fwd ad@v0.5
aht=-ah
turn aht
goto "label28"
label "label27"
x=x-0.5
ah=atan2(y,x)
eval th
ah=ah+th
ah=180*(ah/pi)
eval ah
ad=sqrt(y*y+x*x)
turn ah
eval ad
fwd ad@v0.5
aht=90-ah
turn aht
turnr 0.5 -90
label "label28"
fwd 0.5
ah=0
ad=100
turn 45
wait 1
vision "ball2 debug=true blue=false smrcl"
wait 1
eval $vis1
ad1=1000
if ($vis1<0.5)>
ad1=sqrt($vis3*$vis3+$vis2*$vis2)
x1=$vis2
y1=$vis3
th1=pi/4
label "label29"
turn -45
wait 1
vision "ball2 debug=true blue=false smrcl"
wait 1
eval $vis1
ad2=1000
if ($vis1<0.5)>
ad2=sqrt($vis3*$vis3+$vis2*$vis2)
x2=$vis2
y2=$vis3
th2=0
label "label210"
turn -45
wait 1
vision "ball2 debug=true blue=false smrcl"
wait 1
eval $vis1
ad3=1000
if ($vis1<0.5)>
ad3=sqrt($vis3*$vis3+$vis2*$vis2)
x3=$vis2
y3=$vis3
th3=-pi/4
label "label211"
turn 45
wait 1
if (ad1>ad) "label212"
x=x1
y=y1
th=th1
label "label212"
if (ad2>ad) "label213"
x=x2
y=y2
th=th2
label "label213"
if (ad3>ad) "label214"
x=x3
y=y3
th=th3
label "label214"
ahc=atan2(y,x)
ahc=ahc+th
eval ahc
if (ahc>0) "label215"
y=y+0.5
ah=atan2(y,x)
eval th
ah=ah+th
ah=180*(ah/pi)
eval ah
ad=0.7*(sqrt(y*y+x*x))
turn ah
eval ad
fwd ad@v0.5
aht=-ah
turn aht
goto "label216"
label "label215"
x=x-0.5
ah=atan2(y,x)
eval th
ah=ah+th
ah=180*(ah/pi)
eval ah
ad=sqrt(y*y+x*x)
turn ah
eval ad
fwd ad@v0.5
aht=90-ah
turn aht
turnr 0.5 -90
label "label216"
fwd 0.5
ah=0
ad=100
turn 45
wait 1
vision "ball2 debug=true blue=false smrcl"
wait 1
eval $vis1
ad1=1000
if ($vis1<0.5)>
ad1=sqrt($vis3*$vis3+$vis2*$vis2)
x1=$vis2
y1=$vis3
th1=pi/4
label "label217"
turn -45
wait 1
vision "ball2 debug=true blue=false smrcl"
wait 1
eval $vis1
ad2=1000
if ($vis1<0.5)>
ad2=sqrt($vis3*$vis3+$vis2*$vis2)
x2=$vis2
y2=$vis3
th2=0
label "label218"
turn -45
wait 1
vision "ball2 debug=true blue=false smrcl"
wait 1
eval $vis1
ad3=1000
if ($vis1<0.5)>
ad3=sqrt($vis3*$vis3+$vis2*$vis2)
x3=$vis2
y3=$vis3
th3=-pi/4
label "label219"
turn 45
wait 1
if (ad1>ad) "label220"
x=x1
y=y1
th=th1
label "label220"
if (ad2>ad) "label221"
x=x2
y=y2
th=th2
label "label221"
if (ad3>ad) "label222"
x=x3
y=y3
th=th3
label "label222"
ahc=atan2(y,x)
ahc=ahc+th
eval ahc
if (ahc>0) "label223"
y=y+0.5
ah=atan2(y,x)
eval th
ah=ah+th
ah=180*(ah/pi)
eval ah
ad=0.7*(sqrt(y*y+x*x))
turn ah
eval ad
fwd ad@v0.5
aht=-ah
turn aht
goto "label224"
label "label223"
x=x-0.5
ah=atan2(y,x)
eval th
ah=ah+th
ah=180*(ah/pi)
eval ah
ad=sqrt(y*y+x*x)
turn ah
eval ad
fwd ad@v0.5
aht=90-ah
turn aht
turnr 0.5 -90
label "label224"
fwd 1
turn 180
Results
The results are highly dependent of the camera calibration. The success rate is high for using only one ball but for more the robot fails in getting around the balls because of accumulated odometry errors.
pi=3.14159265
ah=0
ad=100
turn 45
wait 1
vision "ball2 debug=true blue=false smrcl"
wait 1
eval $vis1
ad1=1000
if ($vis1<0.5)>
ad1=sqrt($vis3*$vis3+$vis2*$vis2)
x1=$vis2
y1=$vis3
th1=pi/4
label "label11"
turn -45
wait 1
vision "ball2 debug=true blue=false smrcl"
wait 1
eval $vis1
ad2=1000
if ($vis1<0.5)>
ad2=sqrt($vis3*$vis3+$vis2*$vis2)
x2=$vis2
y2=$vis3
th2=0
label "label12"
turn -45
wait 1
vision "ball2 debug=true blue=false smrcl"
wait 1
eval $vis1
ad3=1000
if ($vis1<0.5)>
ad3=sqrt($vis3*$vis3+$vis2*$vis2)
x3=$vis2
y3=$vis3
th3=-pi/4
label "label13"
turn 45
wait 1
if (ad1>ad) "label14"
x=x1
y=y1
th=th1
label "label14"
if (ad2>ad) "label15"
x=x2
y=y2
th=th2
label "label15"
if (ad3>ad) "label16"
x=x3
y=y3
th=th3
label "label16"
ahc=atan2(y,x)
ahc=ahc+th
eval ahc
if (ahc>0) "label17"
y=y+0.5
ah=atan2(y,x)
eval th
ah=ah+th
ah=180*(ah/pi)
eval ah
ad=0.7*(sqrt(y*y+x*x))
turn ah
eval ad
fwd ad@v0.5
aht=-ah
turn aht
goto "label18"
label "label17"
x=x-0.5
ah=atan2(y,x)
eval th
ah=ah+th
ah=180*(ah/pi)
eval ah
ad=sqrt(y*y+x*x)
turn ah
eval ad
fwd ad@v0.5
aht=90-ah
turn aht
turnr 0.5 -90
label "label18"
fwd 0.5
ah=0
ad=100
turn 45
wait 1
vision "ball2 debug=true blue=false smrcl"
wait 1
eval $vis1
ad1=1000
if ($vis1<0.5)>
ad1=sqrt($vis3*$vis3+$vis2*$vis2)
x1=$vis2
y1=$vis3
th1=pi/4
label "label19"
turn -45
wait 1
vision "ball2 debug=true blue=false smrcl"
wait 1
eval $vis1
ad2=1000
if ($vis1<0.5)>
ad2=sqrt($vis3*$vis3+$vis2*$vis2)
x2=$vis2
y2=$vis3
th2=0
label "label110"
turn -45
wait 1
vision "ball2 debug=true blue=false smrcl"
wait 1
eval $vis1
ad3=1000
if ($vis1<0.5)>
ad3=sqrt($vis3*$vis3+$vis2*$vis2)
x3=$vis2
y3=$vis3
th3=-pi/4
label "label111"
turn 45
wait 1
if (ad1>ad) "label112"
x=x1
y=y1
th=th1
label "label112"
if (ad2>ad) "label113"
x=x2
y=y2
th=th2
label "label113"
if (ad3>ad) "label114"
x=x3
y=y3
th=th3
label "label114"
ahc=atan2(y,x)
ahc=ahc+th
eval ahc
if (ahc>0) "label115"
y=y+0.5
ah=atan2(y,x)
eval th
ah=ah+th
ah=180*(ah/pi)
eval ah
ad=0.7*(sqrt(y*y+x*x))
turn ah
eval ad
fwd ad@v0.5
aht=-ah
turn aht
goto "label116"
label "label115"
x=x-0.5
ah=atan2(y,x)
eval th
ah=ah+th
ah=180*(ah/pi)
eval ah
ad=sqrt(y*y+x*x)
turn ah
eval ad
fwd ad@v0.5
aht=90-ah
turn aht
turnr 0.5 -90
label "label116"
fwd 0.5
ah=0
ad=100
turn 45
wait 1
vision "ball2 debug=true blue=false smrcl"
wait 1
eval $vis1
ad1=1000
if ($vis1<0.5)>
ad1=sqrt($vis3*$vis3+$vis2*$vis2)
x1=$vis2
y1=$vis3
th1=pi/4
label "label117"
turn -45
wait 1
vision "ball2 debug=true blue=false smrcl"
wait 1
eval $vis1
ad2=1000
if ($vis1<0.5)>
ad2=sqrt($vis3*$vis3+$vis2*$vis2)
x2=$vis2
y2=$vis3
th2=0
label "label118"
turn -45
wait 1
vision "ball2 debug=true blue=false smrcl"
wait 1
eval $vis1
ad3=1000
if ($vis1<0.5)>
ad3=sqrt($vis3*$vis3+$vis2*$vis2)
x3=$vis2
y3=$vis3
th3=-pi/4
label "label119"
turn 45
wait 1
if (ad1>ad) "label120"
x=x1
y=y1
th=th1
label "label120"
if (ad2>ad) "label121"
x=x2
y=y2
th=th2
label "label121"
if (ad3>ad) "label122"
x=x3
y=y3
th=th3
label "label122"
ahc=atan2(y,x)
ahc=ahc+th
eval ahc
if (ahc>0) "label123"
y=y+0.5
ah=atan2(y,x)
eval th
ah=ah+th
ah=180*(ah/pi)
eval ah
ad=0.7*(sqrt(y*y+x*x))
turn ah
eval ad
fwd ad@v0.5
aht=-ah
turn aht
goto "label124"
label "label123"
x=x-0.5
ah=atan2(y,x)
eval th
ah=ah+th
ah=180*(ah/pi)
eval ah
ad=sqrt(y*y+x*x)
turn ah
eval ad
fwd ad@v0.5
aht=90-ah
turn aht
turnr 0.5 -90
label "label124"
fwd 1.5
turn 180
ah=0
ad=100
turn 45
wait 1
vision "ball2 debug=true blue=false smrcl"
wait 1
eval $vis1
ad1=1000
if ($vis1<0.5)>
ad1=sqrt($vis3*$vis3+$vis2*$vis2)
x1=$vis2
y1=$vis3
th1=pi/4
label "label21"
turn -45
wait 1
vision "ball2 debug=true blue=false smrcl"
wait 1
eval $vis1
ad2=1000
if ($vis1<0.5)>
ad2=sqrt($vis3*$vis3+$vis2*$vis2)
x2=$vis2
y2=$vis3
th2=0
label "label22"
turn -45
wait 1
vision "ball2 debug=true blue=false smrcl"
wait 1
eval $vis1
ad3=1000
if ($vis1<0.5)>
ad3=sqrt($vis3*$vis3+$vis2*$vis2)
x3=$vis2
y3=$vis3
th3=-pi/4
label "label23"
turn 45
wait 1
if (ad1>ad) "label24"
x=x1
y=y1
th=th1
label "label24"
if (ad2>ad) "label25"
x=x2
y=y2
th=th2
label "label25"
if (ad3>ad) "label26"
x=x3
y=y3
th=th3
label "label26"
ahc=atan2(y,x)
ahc=ahc+th
eval ahc
if (ahc>0) "label27"
y=y+0.5
ah=atan2(y,x)
eval th
ah=ah+th
ah=180*(ah/pi)
eval ah
ad=0.7*(sqrt(y*y+x*x))
turn ah
eval ad
fwd ad@v0.5
aht=-ah
turn aht
goto "label28"
label "label27"
x=x-0.5
ah=atan2(y,x)
eval th
ah=ah+th
ah=180*(ah/pi)
eval ah
ad=sqrt(y*y+x*x)
turn ah
eval ad
fwd ad@v0.5
aht=90-ah
turn aht
turnr 0.5 -90
label "label28"
fwd 0.5
ah=0
ad=100
turn 45
wait 1
vision "ball2 debug=true blue=false smrcl"
wait 1
eval $vis1
ad1=1000
if ($vis1<0.5)>
ad1=sqrt($vis3*$vis3+$vis2*$vis2)
x1=$vis2
y1=$vis3
th1=pi/4
label "label29"
turn -45
wait 1
vision "ball2 debug=true blue=false smrcl"
wait 1
eval $vis1
ad2=1000
if ($vis1<0.5)>
ad2=sqrt($vis3*$vis3+$vis2*$vis2)
x2=$vis2
y2=$vis3
th2=0
label "label210"
turn -45
wait 1
vision "ball2 debug=true blue=false smrcl"
wait 1
eval $vis1
ad3=1000
if ($vis1<0.5)>
ad3=sqrt($vis3*$vis3+$vis2*$vis2)
x3=$vis2
y3=$vis3
th3=-pi/4
label "label211"
turn 45
wait 1
if (ad1>ad) "label212"
x=x1
y=y1
th=th1
label "label212"
if (ad2>ad) "label213"
x=x2
y=y2
th=th2
label "label213"
if (ad3>ad) "label214"
x=x3
y=y3
th=th3
label "label214"
ahc=atan2(y,x)
ahc=ahc+th
eval ahc
if (ahc>0) "label215"
y=y+0.5
ah=atan2(y,x)
eval th
ah=ah+th
ah=180*(ah/pi)
eval ah
ad=0.7*(sqrt(y*y+x*x))
turn ah
eval ad
fwd ad@v0.5
aht=-ah
turn aht
goto "label216"
label "label215"
x=x-0.5
ah=atan2(y,x)
eval th
ah=ah+th
ah=180*(ah/pi)
eval ah
ad=sqrt(y*y+x*x)
turn ah
eval ad
fwd ad@v0.5
aht=90-ah
turn aht
turnr 0.5 -90
label "label216"
fwd 0.5
ah=0
ad=100
turn 45
wait 1
vision "ball2 debug=true blue=false smrcl"
wait 1
eval $vis1
ad1=1000
if ($vis1<0.5)>
ad1=sqrt($vis3*$vis3+$vis2*$vis2)
x1=$vis2
y1=$vis3
th1=pi/4
label "label217"
turn -45
wait 1
vision "ball2 debug=true blue=false smrcl"
wait 1
eval $vis1
ad2=1000
if ($vis1<0.5)>
ad2=sqrt($vis3*$vis3+$vis2*$vis2)
x2=$vis2
y2=$vis3
th2=0
label "label218"
turn -45
wait 1
vision "ball2 debug=true blue=false smrcl"
wait 1
eval $vis1
ad3=1000
if ($vis1<0.5)>
ad3=sqrt($vis3*$vis3+$vis2*$vis2)
x3=$vis2
y3=$vis3
th3=-pi/4
label "label219"
turn 45
wait 1
if (ad1>ad) "label220"
x=x1
y=y1
th=th1
label "label220"
if (ad2>ad) "label221"
x=x2
y=y2
th=th2
label "label221"
if (ad3>ad) "label222"
x=x3
y=y3
th=th3
label "label222"
ahc=atan2(y,x)
ahc=ahc+th
eval ahc
if (ahc>0) "label223"
y=y+0.5
ah=atan2(y,x)
eval th
ah=ah+th
ah=180*(ah/pi)
eval ah
ad=0.7*(sqrt(y*y+x*x))
turn ah
eval ad
fwd ad@v0.5
aht=-ah
turn aht
goto "label224"
label "label223"
x=x-0.5
ah=atan2(y,x)
eval th
ah=ah+th
ah=180*(ah/pi)
eval ah
ad=sqrt(y*y+x*x)
turn ah
eval ad
fwd ad@v0.5
aht=90-ah
turn aht
turnr 0.5 -90
label "label224"
fwd 1
turn 180
Results
The results are highly dependent of the camera calibration. The success rate is high for using only one ball but for more the robot fails in getting around the balls because of accumulated odometry errors.
Subscribe to:
Posts (Atom)