def main():
"""
Get an image from the chosen video source and then detect the energy
cores from the image. Finally print the coordinates of the found
energy cores.
"""
get_image_func = select_video_source(VIDEO_SOURCE)
while True:
frame = get_image_func()
if frame is None:
continue
cv2.imshow('frame', frame)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
pos_ecore_positions = image_to_center_points(frame,
POS_ECORE_LOW_COLOR,
POS_ECORE_HIGH_COLOR,
'Positive Energy Cores')
neg_ecore_positions = image_to_center_points(frame,
NEG_ECORE_LOW_COLOR,
NEG_ECORE_HIGH_COLOR,
'Negative Energy Cores')
print_core_positions(pos_ecore_positions, neg_ecore_positions)
def main():
"""
Get an image from the chosen video source and then detect the robots
from the image. Finally print the coordinates of the found robots.
"""
get_image_func = select_video_source(VIDEO_SOURCE)
while True:
# Capture stream frame by frame
frame = get_image_func()
if frame is None:
continue
corners, detected_ids, rejected_img_points = \
aruco.detectMarkers(frame,
ARUCO_DICT,
parameters=ARUCO_DETECTER_PARAMETERS)
rvecs, tvecs, _ = aruco.estimatePoseSingleMarkers(
corners, SIZE_OF_MARKER, MTX, DIST)
if tvecs is not None and rvecs is not None:
imaxis = aruco.drawDetectedMarkers(frame, corners, detected_ids)
for i, _ in enumerate(tvecs):
aruco.drawAxis(imaxis, MTX, DIST, rvecs[i], tvecs[i],
SIZE_OF_MARKER)
cv2.imshow('frame', imaxis)
transforms = aruco_poses_to_transforms(detected_ids=detected_ids,
corners=corners,
rvecs=rvecs)
print_transforms(transforms)
else:
cv2.imshow('frame', frame)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
def main():
"""
Get an image from the chosen video source and then detect the robots
from the image. Finally print the coordinates of the found robots.
"""
get_image_func = select_video_source(VIDEO_SOURCE)
i = 0
LIMIT = 25
startposx = 250
startposy = 250
e_startposy = 900
e_startposx = 900
target = {'x': 540, 'y': 540}
speed = 45
'''
STATE 0 = jahtaa palloja
STATE 1 = menossa vihollisen maaliin
STATE 2 = menossa omaan maaliin
SCRAPPED STATE 3 = pakoon
'''
CHASE_BALLS = 0
MOVE_TO_ENEMY_GOAL = 1
MOVE_TO_OWN_GOAL = 2
UNUSED_STATE = 3
state = MOVE_TO_ENEMY_GOAL
while True:
LIMIT = LIMIT + 1
# Capture stream frame by frame
frame = get_image_func()
if frame is None:
continue
corners, detected_ids, rejected_img_points = \
aruco.detectMarkers(frame,
ARUCO_DICT,
parameters=ARUCO_DETECTER_PARAMETERS)
rvecs, tvecs, _ = aruco.estimatePoseSingleMarkers(
corners, SIZE_OF_MARKER, MTX, DIST)
#frame = get_image_func()
if frame is None:
continue
cv2.imshow('frame', frame)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
pos_ecore_positions = image_to_center_points(frame,
POS_ECORE_LOW_COLOR,
POS_ECORE_HIGH_COLOR,
'Positive Energy Cores')
neg_ecore_positions = image_to_center_points(frame,
NEG_ECORE_LOW_COLOR,
NEG_ECORE_HIGH_COLOR,
'Negative Energy Cores')
print_core_positions(pos_ecore_positions, neg_ecore_positions)
if tvecs is not None and rvecs is not None:
imaxis = aruco.drawDetectedMarkers(frame, corners, detected_ids)
for i, _ in enumerate(tvecs):
aruco.drawAxis(imaxis, MTX, DIST, rvecs[i], tvecs[i],
SIZE_OF_MARKER)
cv2.imshow('frame', imaxis)
transforms = aruco_poses_to_transforms(detected_ids=detected_ids,
corners=corners,
rvecs=rvecs)
print_transforms(transforms)
for id in transforms.keys():
if id == 11:
print("STATE IS " + str(state))
robot = {
"x": fix_x(transforms[id]['position'][0]),
"y": transforms[id]['position'][1],
"rotation": fix_degrees(transforms[id]['rotation'])
}
max_core = None
min_dist = 90000000
'''
Jos löytyy keltaisia palloja JA ollaan jahtaamassa palloja
'''
if neg_ecore_positions and state == CHASE_BALLS: #neg_ecore POSITIVE, YELLOW BALLS
for core in neg_ecore_positions:
coreX, coreY, alfa = transform_target(
robot, fix_x(core[0]), core[1])
core_dist = coreX * coreX + coreY * coreY # a^2 + b^2 = c^2, no need to sqrt for distance comparison
if core_dist < min_dist:
max_core = core
min_dist = core_dist
'''
Joka 25. frame tsekkaa lähimmän (KEL)pallon lokaation
jos lähin pallo on omassa maalissa
lähtee kulkemaan vastustajan maaliin
'''
if LIMIT % 25 == 0:
target = {
'x': fix_x(max_core[0]),
'y': max_core[1]
}
'''
target['x'], target['y'] = pointbehindball(ball, startposx, startposy, 1)
a = (robot['x'] - target['x']) * (robot['x'] - target['x'])
b = (robot['y'] - target['y']) * (robot['y'] - target['y'])
c = math.sqrt(a + b)
if (c < 300):
target['x'] = ball['x']
target['y'] = ball['y']
'''
#print("TARGET X ON " + str(target['x']) + "\n VS STARTPOS " + str(fix_x(startposx)) + "TARGET Y ON " + str(target['y']) + "\n VS STARTPOSY " + str(startposy))
if fix_x(startposx) > 540:
if target['x'] > fix_x(
startposx) and target['y'] < startposy:
state = MOVE_TO_ENEMY_GOAL
else:
if target['x'] < fix_x(
startposx) and target['y'] > startposy:
state = MOVE_TO_ENEMY_GOAL
elif pos_ecore_positions and state == 0: #pos_core NEGATIVE, YELLOW BALLS Jos löytyy punaisia palloja JA ollaan jahtaamassa palloja
for core in pos_ecore_positions:
coreX, coreY, alfa = transform_target(
robot, fix_x(core[0]), core[1])
core_dist = coreX * coreX + coreY * coreY # a^2 + b^2 = c^2, no need to sqrt for distance comparison
if core_dist < min_dist:
max_core = core
min_dist = core_dist
'''
Joka 25. frame tsekkaa lähimmän (PUN)pallon lokaation
jos lähin pallo on vihollisen maalissa
lähtee kulkemaan omaan maaliin
'''
if LIMIT % 25 == 0:
target = {
'x': fix_x(max_core[0]),
'y': max_core[1]
}
#print("TARGET X ON " + str(target['x']) + "\n VS STARTPOS " + str(fix_x(startposx)) + "TARGET Y ON " + str(target['y']) + "\n VS STARTPOSY " + str(startposy))
if fix_x(startposx) > 540:
if target['x'] < fix_x(e_startposx) and target[
'y'] > e_startposy:
state = MOVE_TO_OWN_GOAL
else:
if target['x'] > fix_x(e_startposx) and target[
'y'] < e_startposy:
state = MOVE_TO_OWN_GOAL
elif state == MOVE_TO_ENEMY_GOAL:
target = {'x': fix_x(e_startposx), 'y': e_startposy}
elif state == MOVE_TO_OWN_GOAL:
target = {'x': fix_x(startposx), 'y': startposy}
#elif state == 3:
#target = {'x': 540, 'y': 540}
targetXX, targetYY, alfa = transform_target(
robot, target['x'], target['y'])
print("target after x:{:.2f} y: {:.2f} alfa:{:.2f}".format(
targetXX, targetYY, alfa))
if alfa < -20:
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
sock.sendto(bytes(f"{speed};-{speed}", "utf-8"),
(IP_11, ROBOT_PORT))
elif alfa > 20:
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
sock.sendto(bytes(f"-{speed};{speed}", "utf-8"),
(IP_11, ROBOT_PORT))
elif targetXX < 0:
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
sock.sendto(bytes(f"-100;-100", "utf-8"),
(IP_11, ROBOT_PORT))
elif targetXX > 0:
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
sock.sendto(bytes(f"100;100", "utf-8"),
(IP_11, ROBOT_PORT))
'''
jos ollaan menossa vastustajan maaliin
jos vastustajan maali on pieni x iso y
jos robotin koordinaatit ovat pienemmät ja isommat
ryhdy jahtaamaan palloja
jos vastustajan maali on iso x pieni y
jos robotit koordinaatit ovat isommat ja pienemmät
ryhdy jahtaamaan palloja
jos ollaan menossa omaan maaliin, sama homma mutta omaan
maaliin verraten.
'''
if state == MOVE_TO_ENEMY_GOAL:
if fix_x(e_startposx) < 540:
if robot['x'] < 2 * fix_x(e_startposx) and robot[
'y'] > e_startposy - 130:
state = CHASE_BALLS
else:
if robot['x'] > fix_x(e_startposx) - 130 and robot[
'y'] < 2 * e_startposy:
state = CHASE_BALLS
elif state == MOVE_TO_OWN_GOAL:
if fix_x(startposx) < 540:
if robot['x'] < 2 * fix_x(startposx) and robot[
'y'] > startposy - 130:
state = CHASE_BALLS
else:
if robot['x'] > fix_x(startposx) - 130 and robot[
'y'] < 2 * startposy:
state = CHASE_BALLS
#elif state == 3:
#if robot['x'] < 500 and robot['y'] > 500:
#state = 0
else:
cv2.imshow('frame', frame)
if cv2.waitKey(1) & 0xFF == ord('q'):
break