162 lines
4.7 KiB
Python
162 lines
4.7 KiB
Python
# goal dist tracking with conic mirror - By: EmaMaker - fri 21 feb 2020
|
|
# Based on:
|
|
# color tracking - By: paolix - ven mag 18 2018
|
|
|
|
# Automatic RGB565 Color Tracking Example
|
|
#
|
|
import sensor, image, time, pyb, math
|
|
from pyb import UART
|
|
uart = UART(3,19200, timeout_char = 1000)
|
|
|
|
def torad(f):
|
|
return (f*math.pi/180) % math.pi
|
|
|
|
#These measures are in centimeters
|
|
FIELD_W = 131
|
|
FIELD_H = 193
|
|
GOALS_DEPTH = 207
|
|
|
|
#Attack 1 means attacking yellow, attack 0 means attacking blue
|
|
ATTACKING = 0
|
|
|
|
# LED Setup ##################################################################
|
|
|
|
red_led = pyb.LED(1)
|
|
green_led = pyb.LED(2)
|
|
blue_led = pyb.LED(3)
|
|
|
|
red_led.off()
|
|
green_led.off()
|
|
blue_led.on()
|
|
##############################################################################
|
|
|
|
|
|
#thresholds = [ (30, 100, 15, 127, 15, 127), # generic_red_thresholds
|
|
# (30, 100, -64, -8, -32, 32), # generic_green_thresholds
|
|
# (0, 15, 0, 40, -80, -20)] # generic_blue_thresholds
|
|
|
|
#thresholds = [ (54, 93, -10, 25, 55, 70), # thresholds yellow goal
|
|
# (30, 45, 1, 40, -60, -19)] # thresholds blue goal
|
|
#
|
|
thresholds = [ (40, 100, -3, 35, 16, 96) , # thresholds yellow goal
|
|
(39, 59, -13, 12, -43, -19)] # thresholds blue goal (6, 31, -15, 4, -35, 0)
|
|
|
|
roi = (0, 6, 318, 152)
|
|
|
|
# Camera Setup ###############################################################
|
|
'''sensor.reset()
|
|
sensor.set_pixformat(sensor.RGB565)
|
|
sensor.set_framesize(sensor.QVGA)
|
|
sensor.skip_frames(time = 2000)
|
|
sensor.set_auto_gain(False) # must be turned off for color tracking
|
|
sensor.set_auto_whitebal(False) # must be turned off for color tracking
|
|
sensor.set_auto_exposure(False, 10000)
|
|
#sensor.set_backlight(1)
|
|
#sensor.set_brightness(+2)
|
|
#sensor.set_windowing(roi)
|
|
clock = time.clock()'''
|
|
|
|
sensor.reset()
|
|
sensor.set_pixformat(sensor.RGB565)
|
|
sensor.set_framesize(sensor.QQVGA)
|
|
sensor.set_contrast(+2)
|
|
sensor.set_saturation(2)
|
|
sensor.set_brightness(-3)
|
|
sensor.set_quality(0)
|
|
sensor.set_auto_exposure(False, 6000)
|
|
sensor.set_auto_gain(True)
|
|
sensor.skip_frames(time = 300)
|
|
|
|
clock = time.clock()
|
|
##############################################################################
|
|
|
|
|
|
# [] list
|
|
# () tupla
|
|
|
|
'''while(True):
|
|
clock.tick()
|
|
img = sensor.snapshot()'''
|
|
|
|
while(True):
|
|
clock.tick()
|
|
|
|
blue_led.off()
|
|
|
|
tt_yellow = [(0,999,0,1)] ## creo una lista di tuple per il giallo, valore x = 999 : non trovata
|
|
tt_blue = [(0,999,0,2)] ## creo una lista di tuple per il blue, valore x = 999 : non trovata
|
|
|
|
img = sensor.snapshot()
|
|
for blob in img.find_blobs(thresholds, pixels_threshold=45, area_threshold=80, merge = True):
|
|
img.draw_rectangle(blob.rect())
|
|
img.draw_cross(blob.cx(), blob.cy())
|
|
|
|
if (blob.code() == 1):
|
|
tt_yellow = tt_yellow + [ (blob.area(),blob.cx(),blob.cy(),blob.code() ) ]
|
|
if (blob.code() == 2):
|
|
tt_blue = tt_blue + [ (blob.area(),blob.cx(),blob.cy(),blob.code() ) ]
|
|
|
|
tt_yellow.sort(key=lambda tup: tup[0]) ## ordino le liste
|
|
tt_blue.sort(key=lambda tup: tup[0]) ## ordino le liste
|
|
|
|
ny = len(tt_yellow)
|
|
nb = len(tt_blue)
|
|
|
|
'''Yellow'''
|
|
area,cx,cy,code = tt_yellow[ny-1]
|
|
cx = img.width() / 2 - cx
|
|
cy = img.height() / 2 - cy
|
|
yAngle = math.pi/2 - math.atan2(cy, cx)
|
|
yDist = math.sqrt(cx*cx + cy*cy)
|
|
string_yellow = "Y"+str(cx)+" | "+str(cy)+" | "+str(yAngle)+" | "+str(yDist)+str(area)+"y"
|
|
#print (string_yellow) # test on serial terminal
|
|
|
|
'''Blue'''
|
|
area,cx,cy,code = tt_blue[nb-1]
|
|
cx = img.width() / 2 - cx
|
|
cy = img.height() / 2 - cy
|
|
bAngle = math.pi/2 - math.atan2(cy, cx)
|
|
bDist = math.sqrt(cx*cx + cy*cy)
|
|
string_blue = "B"+str(cx)+" | "+str(cy)+" | |"+str(bAngle)+" | "+str(bDist)+str(area)+"b"
|
|
#print (string_blue) # test on serial terminal
|
|
|
|
#Now calculate distance and position
|
|
#Goal 1 is the one in front of the robot
|
|
#Goal 2 is the one facing the back of the robot
|
|
|
|
#convert in [0, 360), to be sure
|
|
bAngle = int(bAngle * 180 / math.pi)
|
|
yAngle = int(yAngle * 180 / math.pi)
|
|
|
|
bAngle = (bAngle + 360) % 360;
|
|
yAngle = (yAngle + 360) % 360;
|
|
|
|
#Now bring it back to [-179, 180]
|
|
if bAngle > 180:
|
|
bAngle = bAngle - 360
|
|
|
|
if yAngle > 180:
|
|
yAngle = yAngle - 360
|
|
|
|
if ATTACKING == 1:
|
|
angle1 = abs(yAngle)
|
|
angle2 = abs(bAngle - 180)
|
|
else:
|
|
angle1 = abs(bAngle)
|
|
angle2 = abs(yAngle - 180)
|
|
|
|
dist1 = (GOALS_DEPTH * math.sin(angle2) ) / (math.sin(angle1+angle2))
|
|
dist2 = (GOALS_DEPTH * math.sin(angle1) ) / (math.sin(angle1+angle2))
|
|
|
|
print("------")
|
|
print(angle1)
|
|
print(angle2)
|
|
print(dist1)
|
|
print(dist2)
|
|
print("------")
|
|
|
|
|
|
#print ("..................................")
|
|
|
|
print(clock.fps())
|