anykey = raw_input("============= Press any key to start =========== \n")
print "================ a key is pressed. Go go go! ========================="
carState = state_begin # begin with "findBall"
isNearBaseLine = True
navigateCount = 0
while not rospy.is_shutdown():
global masterFindBall, isSlaverAttacking, isNearBaseLine, navigateCount
if carState == state_begin:
print "--------------------- <state> begin the battle ---------------------"
# motion_control.PointMove(0.9, -0.55, 0) # begin to move to a point.
# motion_control.PointMove(0.5, 0, 0)
motion_control.PointMove(1.35, -0.65, 0)
isNearBaseLine = False
carState = state_findBall
else:
if carState == state_findBall:
bottomImg = findBallDoorTogether()
# print "--------------------- <state> find ball ---------------------"
if isBallNear(bottomImg):
isNearBaseLine = False
motion_control.PointMove(0.4, 0, 0)
state_begin = 'state_begin'
state_findBall = 'state_findBall'
state_changePosition = 'state_changePosition'
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
state_changePosition = 'state_changePosition'
anykey = raw_input("============= Press any key to start =========== \n")
print "================ a key is pressed. Go go go! ========================="
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:
anykey = raw_input("============= Press any key to start =========== \n")
print "================ a key is pressed. Go go go! ========================="
carState = state_begin # begin with "findBall"
isNearBaseLine = True
while not rospy.is_shutdown():
global masterFindBall, isSlaverAttacking, isNearBaseLine
if carState == state_begin:
print "--------------------- <state> begin the battle ---------------------"
# motion_control.PointMove(0.8, -0.55, 0) # begin to move to a point.
# motion_control.PointMove(0.5, 0, 0)
motion_control.PointMove(0.4, 0.4, 0)
# motion_control.rotateTo2(90)
motion_control.rotateTo2(179)
# motion_control.rotateTo2(-90)
motion_control.rotateTo2(0)
isNearBaseLine = True
carState = state_findBall
else:
if carState == state_findBall:
result, bottomImg = findBallDoorTogether()
# print "--------------------- <state> find ball ---------------------"
anykey = raw_input("============= Press any key to start =========== \n")
print "================ a key is pressed. Go go go! ========================="
carState = state_begin # begin with "findBall"
isNearBaseLine = False
navigateCount = 0
while not rospy.is_shutdown():
global masterFindBall, isSlaverAttacking, isNearBaseLine, navigateCount
global lastAttacker
if carState == state_begin:
print "--------------------- <state> begin the battle ---------------------"
motion_control.PointMove(0.9, -0.55, 0) # begin to move to a point.
motion_control.PointMove(0.5, 0, 0) # begin to move to a point.
carState = state_findBall
else:
# print "Find ball..."
# img = recvpic(host)
# img = recvpic_onconnect(sock)
# img = getPicture()
result, bottomImg = findBallDoorTogether()
# print "Time used in 'getPicture': ", (int)((time.time()-t)*1000)
if carState == state_findBall:
# print "--------------------- <state> find ball ---------------------"
if isBallNear(bottomImg):
# print "Master is near ball but slaver is coming......"
masterFindBall = True
targetPointX, targetPointY = 0,0
print "================ waiting for odom to reset ========================="
anykey = raw_input("============= Press any key to start =========== \n")
print "================ a key is pressed ========================="
start_time = time.time()
isMyHalf = True
moveBackCount = 0
while not rospy.is_shutdown():
if carState == state_begin:
print "--------------------- <state> begin the battle ---------------------"
motion_control.PointMove(0.1, -0.1, 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_top()
result = findElement_top(img)
if len(result['football']) > 1:
ballX, ballY = result['football']
print "Find football at: ", ballX, ballY
if abs(ballY) < 0.1: # the ball is in the front
print "Robot is facing the ball."
motion_control.PointMove(ballX + 0.3, 0)
