def closeball2(threshold, close1):
redball000.lookat(robotIP, PORT)
while (1):
size, left, top, cam, isfind = redball000.findball(robotIP, PORT)
if (isfind == 1):
if (top >= threshold):
closeball1(close1)
break
elif (-60 <= top < 20):
x = 0.02
y = 0.0
theta = 0
motionProxy.moveTo(x, y, theta)
time.sleep(1.8)
closeball1(close1)
elif (20 <= top < threshold):
x = 0.02
y = 0.0
theta = 0.0
motionProxy.moveTo(x, y, theta)
time.sleep(1.8)
closeball1(27)
elif (top < -60):
x = 0.07
y = 0.0
theta = 0.0
motionProxy.moveTo(x, y, theta)
time.sleep(1.8)
else:
pass
def oldclose(threshold):
LEFT_ = 0
redball000.lookat(robotIP, PORT)
count = 1
flag_p = 0
flag_n = 0
while (1):
size, left, top, cam, isfind = redball000.findball(robotIP, PORT)
if (isfind == 1):
if (cam == 1):
if (left <= threshold) & (left >= -threshold):
break
elif (left > threshold):
flag_p = 1
if ((flag_p == 1) & (flag_n == 1)):
count = count + 1
flag_n = 0
x = 0.0
y = 0.0
theta = -1 * math.pi / (8 * (2 * count))
motionProxy.moveTo(x, y, theta)
time.sleep(1.5)
elif (left < -threshold):
flag_n = 1
if ((flag_p == 1) & (flag_n == 1)):
count = count + 1
flag_p = 0
x = 0.0
y = 0.0
theta = math.pi / (8 * (2 * count))
motionProxy.moveTo(x, y, theta)
time.sleep(1.5)
else:
Movex(0.25)
if (left < 20):
x = 0.0
y = 0.0
theta = math.pi / 16
motionProxy.moveTo(x, y, theta)
time.sleep(1.5)
elif (left > 20):
x = 0.0
y = 0.0
theta = -1 * math.pi / 16
motionProxy.moveTo(x, y, theta)
time.sleep(1.5)
else:
if (LEFT_ == 0):
x = 0.0
y = 0.0
theta = math.pi / 8
motionProxy.moveTo(x, y, theta)
else:
x = 0.0
y = 0.0
theta = -math.pi / 8
motionProxy.moveTo(x, y, theta)
def far_ball():
close1_ = 23
topdistance = 30
Ldistance = 30
threshold = 20
while (1):
isfindLand, size, x, ss = DetecteLandmark_my()
if (isfindLand == 0):
count = count + 1
Movey(0.06)
redball000.lookat(robotIP, PORT)
elif (((x - Ldistance) < threshold) & ((x - Ldistance) > -threshold)):
return size
elif (x - Ldistance < -threshold):
Movey(-0.02)
closeball1(17)
closeball2(topdistance, close1_)
def runtohitball90(threshold, topdistance, ditance):
# turn head
# detecteland
# Movex(-0.08)
close1_ = 23
count = 0
while (1):
names = ["HeadPitch", "HeadYaw"]
angleLists = [0 * almath.TO_RAD, 90.0 * almath.TO_RAD]
timeLists = [1.0, 2.0]
isAbsolute = True
motionProxy.angleInterpolation(names, angleLists, timeLists,
isAbsolute)
time.sleep(2.0)
isfindLand, x, y, z, d = DetecteLandmark()
time.sleep(2.0)
# print "landmark"
if (isfindLand == 0):
# print x, threshold + ditance, ditance - threshold
count = count + 1
Movey(0.10)
redball000.lookat(robotIP, PORT)
closeball1(17)
closeball2(topdistance, close1_)
elif (((x - ditance) < threshold) & ((x - ditance) > -threshold)):
# print x,threshold+ditance,ditance-threshold
return x, y, z, d
elif (x - ditance < -threshold):
# print x, threshold + ditance, ditance - threshold
Movey(-0.02)
redball000.lookat(robotIP, PORT)
# *************************************
closeball1(17)
closeball2(topdistance, close1_)
else:
count = count + 1
Movey(0.05)
redball000.lookat(robotIP, PORT)
closeball1(17)
closeball2(topdistance, close1_)
def newruntohitball(threshold, topdistance, ditance):
# turn head
# detecteland
# Movex(-0.08)
close1_ = 23
count = 0
fivefindlandmark = 0
while (1):
names = ["HeadPitch", "HeadYaw"]
angleLists = [0 * almath.TO_RAD, 90.0 * almath.TO_RAD]
timeLists = [1.0, 2.0]
isAbsolute = True
motionProxy.angleInterpolation(names, angleLists, timeLists,
isAbsolute)
time.sleep(2.0)
isfindLand, x, y, z, d = DetecteLandmark()
time.sleep(2.0)
# print "landmark"
# 90 degre cannot find
if (isfindLand == 0):
for i in range(0, 5):
# ***********************
names = ["HeadPitch", "HeadYaw"]
angleLists = [
0.0 * almath.TO_RAD, (60.0 - i * 30.0) * almath.TO_RAD
]
timeLists = [1.0, 2.0]
isAbsolute = True
motionProxy.angleInterpolation(names, angleLists, timeLists,
isAbsolute)
time.sleep(1.0)
# *****************************
isfindLand, x, y, z, d = DetecteLandmark()
# find landmark
if (isfindLand == 1):
if (i == 0):
t_ = 0.08
elif (i == 1):
t_ = 0.12
elif (i == 2):
t_ = 0.16
elif (i == 3):
t_ = 0.20
elif (i == 4):
t_ = 0.24
elif (i == 5):
t_ = 0.30
Movey(t_)
Movet(-0.75 * (math.atan(t_ / ditance)))
fivefindlandmark = 1
break
else:
pass
if (fivefindlandmark == 0):
t_ = 0.30
Movey(t_)
Movet(0.75 * math.atan(t_ / ditance))
closeball1(20)
closeball2(topdistance, close1_)
elif (((x - ditance) < threshold) & ((x - ditance) > -threshold)):
# print x,threshold+ditance,ditance-threshold
return x, y, z, d
elif (x - ditance < -threshold):
# print x, threshold + ditance, ditance - threshold
Movey(-0.04)
redball000.lookat(robotIP, PORT)
# *************************************
closeball1(17)
closeball2(topdistance, close1_)
else:
count = count + 1
Movey(0.04)
redball000.lookat(robotIP, PORT)
closeball1(17)
closeball2(topdistance, close1_)
def closeball1_right(threshold):
LEFT_ = 0
redball000.lookat(robotIP, PORT)
count = 1
count0 = 0
flag_p = 0
flag_n = 0
while (1):
if (count0 == 10):
Movey(0.5)
size, left, top, cam, isfind = redball000.findball(robotIP, PORT)
if (isfind == 1):
if (cam == 1):
if (left <= threshold) & (left >= -threshold):
break
elif (left > threshold):
flag_p = 1
if ((flag_p == 1) & (flag_n == 1)):
count = count + 1
flag_n = 0
x = 0.0
y = 0.0
theta = -1 * math.pi / (10 * (2 * count))
motionProxy.moveTo(x, y, theta)
time.sleep(1.5)
elif (left < -threshold):
flag_n = 1
if ((flag_p == 1) & (flag_n == 1)):
count = count + 1
flag_p = 0
x = 0.0
y = 0.0
theta = math.pi / (10 * (2 * count))
motionProxy.moveTo(x, y, theta)
time.sleep(1.5)
else:
if (top <= -10):
if (left < 30):
x = 0.0
y = 0.0
theta = math.pi / 15
motionProxy.moveTo(x, y, theta)
time.sleep(1.5)
elif (left > 30):
x = 0.0
y = 0.0
theta = -1 * math.pi / 15
motionProxy.moveTo(x, y, theta)
time.sleep(1.5)
Movex(0.45)
elif (-10 <= top <= 50):
if (left < 25):
x = 0.0
y = 0.0
theta = math.pi / 15
motionProxy.moveTo(x, y, theta)
time.sleep(1.5)
elif (left > 25):
x = 0.0
y = 0.0
theta = -1 * math.pi / 15
motionProxy.moveTo(x, y, theta)
time.sleep(1.5)
Movex(0.30)
else:
if (left < 25):
x = 0.0
y = 0.0
theta = math.pi / 15
motionProxy.moveTo(x, y, theta)
time.sleep(1.5)
elif (left > 25):
x = 0.0
y = 0.0
theta = -1 * math.pi / 15
motionProxy.moveTo(x, y, theta)
time.sleep(1.5)
Movex(0.15)
else:
if (LEFT_ == 0):
count0 = count0 + 1
x = 0.0
y = 0.0
theta = math.pi / 8
motionProxy.moveTo(x, y, theta)
else:
count0 = count0 + 1
x = 0.0
y = 0.0
theta = -math.pi / 8
motionProxy.moveTo(x, y, theta)
def newruntohitball_far(threshold, topdistance, ditance):
xx = 0.0
i = 0
close1_ = 27
count = 0
fivefindlandmark = 0
while (1):
names = ["HeadPitch", "HeadYaw"]
angleLists = [0 * almath.TO_RAD, 90.0 * almath.TO_RAD]
timeLists = [1.0, 2.0]
isAbsolute = True
motionProxy.angleInterpolation(names, angleLists, timeLists,
isAbsolute)
time.sleep(2.0)
isfindLand, size, xx, ss = DetecteLandmark_my()
# print xx,threshold+ditance,ditance-threshold
# time.sleep(2.0)
# print "landmark"
# 90 degre cannot find
if (isfindLand == 0):
while (1):
# ***********************
names = ["HeadPitch", "HeadYaw"]
angleLists = [
0.0 * almath.TO_RAD, (90.0 - i * 30.0) * almath.TO_RAD
]
timeLists = [1.0, 2.0]
isAbsolute = True
motionProxy.angleInterpolation(names, angleLists, timeLists,
isAbsolute)
time.sleep(1.0)
# *****************************
isfindLand, size, xx, ss = DetecteLandmark_my()
# find landmark
if (isfindLand == 1):
if (i == 0):
t_ = 0.08
elif (i == 1):
t_ = 0.12
elif (i == 2):
t_ = 0.16
elif (i == 3):
t_ = 0.20
elif (i == 4):
t_ = 0.24
elif (i == 5):
t_ = 0.30
Movey(t_)
Movet(-(math.atan(t_ / ditance) + math.pi / 16))
fivefindlandmark = 1
i = i + 1
i = 0
break
else:
i = i + 1
pass
if (i == 6):
i = 0
break
if (fivefindlandmark == 0):
t_ = 0.30
Movey(t_)
Movet(-(math.atan(t_ / ditance) + math.pi / 16))
closeball1(close1_)
closeball2(topdistance, close1_)
elif (((xx - ditance) > threshold) & ((xx - ditance) < -threshold)):
return size, xx
else:
count = count + 1
Movey(0.04)
redball000.lookat(robotIP, PORT)
closeball1(close1_)
closeball2(topdistance, close1_)
