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.