# 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':