# motion_control.rotateTo2(0)
motion_control.PointMove(0.2, 0, 0)
else:
print "<BaskLine> Just in position, do not move back or forward. Find ball again."
pass
if doorX > 1.7:
isNearBaseLine = True
if isNearBaseLine == True:
print "Move around the line ..., Times: ", navigateCount
if navigateCount == 0:
pass
elif navigateCount == 1:
motion_control.rotateTo2(90)
motion_control.PointMove(0.5, 0, 0)
elif navigateCount == 2 or navigateCount == 3:
motion_control.rotateTo2(-90)
motion_control.PointMove(0.5, 0, 0)
elif navigateCount == 4:
motion_control.rotateTo2(90)
motion_control.PointMove(0.5, 0, 0)
navigateCount = 0
else:
print "Error"
key = raw_input("Error---\n")
navigateCount += 1
else:
navigateCount = 0
anykey = raw_input("============= Press any key to start =========== \n")
print "================ a key is pressed. Go go go! ========================="
carState = state_begin # begin with "findBall"
while not rospy.is_shutdown():
global masterFindBall, isSlaverAttacking
if carState == state_begin:
print "--------------------- <state> begin the battle ---------------------"
motion_control.PointMove(0.8, -0.5, 0) # begin to move to a point.
carState = state_findBall
else:
print "Find ball..."
motion_control.rotateTo2(0)
img = getPicture()
if carState == state_findBall:
print "--------------------- <state> find ball ---------------------"
if isBallNear(img):
print "mask is near ball but slaver is coming......"
masterFindBall = True
motion_control.PointMove(0.4, 0, 0)
continue
print "The ball is NOT near -- by HSV. Use yolo now."
result = findElement(img, 'ball')
if len(result['football']) > 1:
masterFindBall = True
ballX, ballY = result['football']
anykey = raw_input("============= Press any key to start =========== \n")
print "================ a key is pressed. Go go go! ========================="
carState = state_begin # begin with "findBall"
while not rospy.is_shutdown():
if carState == state_begin:
print "--------------------- <state> begin the battle ---------------------"
motion_control.PointMove(0.8, -0.5, 0) # begin to move to a point.
carState = state_findBall
elif carState == state_findBall:
print "--------------------- <state> find ball ---------------------"
motion_control.rotateTo2(0) # to face the front
img = getPicture()
if isBallNear(img):
motion_control.PointMove(0.3, 0, 0)
continue
print "The ball is NOT near -- by HSV. Use yolo now."
if len(result['football']) > 1:
masterFindBall = True
ballX, ballY = result['football']
print "Find football at: ", ballX, ballY
# anykey = raw_input("============= Press any key to Move to ball... =========== \n")
if abs(ballY) < 0.1: # the ball is in the front
print "Robot is facing the ball. Move forward."
motion_control.PointMove(ballX + 0.4, 0, 0)
else:
carState = state_begin # begin with "findBall"
while not rospy.is_shutdown():
global masterFindBall, slaverFindBall, masterInstruction, slaverInstruction
'''
anykey = raw_input("============= Input slaver find ball 0/1 =========== \n")
slaverFindBall = int(anykey)
slaver_publish()
'''
if carState == state_begin:
print "--------------------- <state> begin the battle ---------------------"
motion_control.PointMove(0.3, 0.4, 0) # begin to move to a point.
motion_control.rotateTo2(90)
motion_control.rotateTo2(179)
motion_control.rotateTo2(-90)
motion_control.rotateTo2(0)
carState = state_findBall
elif carState == state_findBall:
print "--------------------- <state> find ball ---------------------"
# anykey = raw_input("============= Press any key to continue =========== \n")
motion_control.rotateTo2(0) # to face the front
img = getPicture()
if isBallNear(img):
slaverFindBall = True
carState = state_begin # begin with "findBall"
isNearBaseLine = True
haveSeeTwoPillars = False
while not rospy.is_shutdown():
global slaverFindBall, slaverInstruction, isNearBaseLine, navigateCount
if carState == state_begin:
print "--------------------- <state> begin the battle ---------------------"
motion_control.PointMove(0.4, 0.4, 0) # begin to move to a point.
# rotate to wait
# motion_control.rotateTo2(0)
# motion_control.rotateTo2(90)
motion_control.rotateTo2(179)
motion_control.rotateTo2(-90)
motion_control.rotateTo2(0)
isNearBaseLine = True
carState = state_findBall
else:
# img = getPicture()
result, bottomImg = findBallDoorTogether()
# print "<----------------> Time used by bottom camera: ", (int)((time.time()-t)*1000), "ms. ?????????"
if carState == state_findBall:
print "--------------------- <state> find ball ---------------------"
if isBallNear(bottomImg):
slaverFindBall = True
slaver_publish()
while slaverInstruction == 'waitForResponse':
