def smallSentry(mag_q):
#search for inverted cones
#track them and return their angle compared to center of robot (-180 to 180)
#possibly implement reverse queue to turn cannon when hitting navigaton error.
camera = recognition.setup()
cannon.init()
lastSeenTime = time.time()
try:
# Function that will look for a cone and fire if found. Used after a magnet is detected.
foundMagnet = None
while foundMagnet != 'Stop':
try:
foundMagnet = mag_q.get(timeout=0.001)
print foundMagnet
except:
pass
print "In small sentry."
targetObjects = recognition.detect(camera, SPHEREFILE)
index = recognition.biggestObject(targetObjects)
if index != -1:
lastSeenTime = time.time()
# Move cannon
if recognition.isCentered(targetObjects[index]):
print "Firing"
cannon.moveV(0.1)
cannon.moveH(-0.03)
navigate.blink()
cannon.firex_small(1)
cannon.moveH(0.03)
cannon.moveV(-0.1)
else:
# Move cannon
biggestObject = targetObjects[index]
xTarg = biggestObject[0] + biggestObject[2] / 2
yTarg = biggestObject[1] + biggestObject[3] / 2
print "Found an object, moving to coordinates, x: " + ` xTarg ` + " y: " + ` yTarg `
cannon.moveTo(xTarg * 1.10, yTarg)
elif time.time() - lastSeenTime > TIMEOUT:
cannon.sweepH(0.08)
time.sleep(0.2)
print "Sweeping"
finally:
camera.close()
print "Ran the smallSentry close"
def scan(nav_q, scan_q):
#search for inverted cones
#track them and return their angle compared to center of robot (-180 to 180)
#possibly implement reverse queue to turn cannon when hitting navigaton error.
camera = recognition.setup()
cannon.init()
lastSeenTime = time.time()
try:
# Function that will look for a cone and fire if found. Used after a magnet is detected.
while True:
print "10"
try:
targetAngle = scan_q.get(timeout=0.001)
print "Cannon Target angle:", targetAngle
cannon.moveAngle(-targetAngle)
except:
pass
print "detect, check, fire or track"
targetObjects = recognition.detect(camera, INVERTEDCONEFILE)
index = recognition.biggestObject(targetObjects)
if index != -1:
lastSeenTime = time.time()
# Move cannon
biggestObject = targetObjects[index]
xTarg = biggestObject[0] + biggestObject[2] / 2
yTarg = biggestObject[1] + biggestObject[3] / 2
print "Found an object, moving to coordinates, x: " + ` xTarg ` + " y: " + ` yTarg `
cannon.moveTo(
xTarg * 1.05, yTarg
) # Might consider multiplying xTarg by a constant, so it overshoots, since robot is moving.
nav_q.put(cannon.getAngle())
print "putting value: " + ` cannon.getAngle() `
elif time.time() - lastSeenTime > TIMEOUT:
cannon.sweepH(0.08)
time.sleep(0.2)
print "Sweeping"
finally:
nav_q.put("STOP")
camera.close()
GPIO.cleanup()
print "Ran the scan close"
def sentryMode():
camera = recognition.setup()
try:
# Function that will look for a cone and fire if found. Used after a magnet is detected.
global DART_COUNT
if DART_COUNT > 0:
cannon.init()
lastSeenTime = time.time()
while DART_COUNT > 0:
print "detect, check, fire or track"
targetObjects = recognition.detect(camera, SPHEREFILE)
index = recognition.biggestObject(targetObjects)
if index != -1:
lastSeenTime = time.time()
# Check if biggest object is in the middle of the screen and fire if it is
if recognition.isCentered(targetObjects[index]):
print "Firing"
cannon.moveV(0.1)
navigate.blink()
cannon.firex(1)
cannon.moveV(-0.1)
DART_COUNT = DART_COUNT - 1
else:
# Move cannon
biggestObject =targetObjects[index]
xTarg = biggestObject[0] + biggestObject[2]/2
yTarg = biggestObject[1] + biggestObject[3]/2
print "Found an object, moving to coordinates, x: " + `xTarg` + " y: " + `yTarg`
cannon.moveTo(xTarg, yTarg)
elif time.time() - lastSeenTime > TIMEOUT:
cannon.sweepH(0.08)
time.sleep(0.2)
print "Out of darts"
finally:
#GPIO.cleanup()
camera.close()
print "Ran the sentry close"