示例#1
0
				if agent.strategy == "Manual":
					manual_cars += 1
			if manual_cars > 1:
				errorMsg = msgHeader + ">1 manual cars enabled."
				print(errorMsg)
				display.error_message(errorMsg)
				time.sleep(2)
				continue
			
			if display.user_defined:
				print(msgHeader+"Detecting cones.")
				display.cone_recognition_screen(True)
				cone_detector = ConeDetector()
				if display.wait_for_confirmation():
					display.cone_recognition_screen()
					cones = vision.getCones()
					newWaypoints = []
					conePairs = []
					# Track-Building Algorithm
					while cones:
						currentCone = cones.pop(0)
						minDist = 99999				# Arbitrarily large number
						pairedIndex = 1
						for posPair in cones:
							currentDist = math.sqrt((currentCone[0] - posPair[0])**2 + (currentCone[1] - posPair[1])**2)
							if minDist > currentDist:
								minDist = currentDist
								pairedIndex = cones.index(posPair)
						conePairs.append((currentCone, cones[pairedIndex]))
						newWaypoints.append((round((currentCone[0] + cones[pairedIndex][0])/2*1.6), round((currentCone[1] + cones[pairedIndex][1])/2*1.6)))
						cones.pop(pairedIndex)