def closeball2(threshold, close1):
#使机器人低下头
redball512_r.lookat(robotIP, PORT)
while (1):
size, left, top, cam, isfind = redball512_r.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 runtohitball90_far(threshold, topdistance, ditance):
# turn head
# detecteland
# Movex(-0.08)
adj = 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, size, x, ss = DetecteLandmark_my()
# print x,threshold+ditance,ditance-threshold
time.sleep(2.0)
# print "landmark"
if (isfindLand == 0):
count = count + 1
Movey(0.12)
redball512_r.lookat(robotIP, PORT)
closeball1(17)
closeball2(topdistance, close1_)
elif (((x - ditance) > threshold) & ((x - ditance) < -threshold)):
return size, x
elif ((x - ditance) < threshold):
Movey(-0.03)
redball512_r.lookat(robotIP, PORT)
# *************************************
closeball1(17)
closeball2(topdistance, close1_)
else:
count = count + 1
Movey(0.04)
redball512_r.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
change = 8.0
while (1):
names = ["HeadPitch", "HeadYaw"]
angleLists = [0 * almath.TO_RAD, (90.0 + change) * 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) + change) * 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)
redball512_r.lookat(robotIP, PORT)
# *************************************
closeball1(17)
closeball2(topdistance, close1_)
else:
count = count + 1
Movey(0.04)
redball512_r.lookat(robotIP, PORT)
closeball1(17)
closeball2(topdistance, close1_)
def closeball1_right(threshold):
LEFT_ = 0
redball512_r.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 = redball512_r.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)