array_values = np.array(frame)
frame_rgb = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB) # Convert from BGR to RGB
image = np.array(frame_rgb).astype(np.float)/255 # Convert from Int8 to float
cs = colour.findCenters(image,tol=0.1,draw=True,frame=frame) # Find colour centres
#print(array_values.shape)
# Display the resulting frame
timestr = "Time: " + time.ctime(time.time()).split(" ")[4]
cv2.putText(frame,timestr, topLeft, font, fontScale,fontColor,lineType)
if not (np.isnan(cs[GREEN][0]) or np.isnan(cs[GREEN][1])):
start = prm.Node(cs[GREEN][0],cs[GREEN][1],N)
if not (np.isnan(cs[BLUE][0]) or np.isnan(cs[BLUE][1])):
ball = prm.Obstacle(cs[BLUE][0],cs[BLUE][1],10)
x,y = prm.pathPlan(graph,start,goal,enemy,ball,k,True,draw=True,w=640,h=480,frame=frame)
cv2.imshow('frame',frame)
out.write(frame)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
# When everything done, release the capture
cap.release()
out.release()
cv2.destroyAllWindows()
def run(bot, reset=False, game_over=False):
ball = cs[GREEN]
canSeeBall = True
# the ball is the goal
if bot == 0:
# Define points
back = cs[WHITE]
front = cs[RED]
start = [(back[0] + front[0]) / 2, (back[1] + front[1]) / 2]
enemy_front = cs[BLUE]
enemy_back = cs[YELLOW]
enemy = [(enemy_back[0] + enemy_front[0]) / 2,
(enemy_back[1] + enemy_front[1]) / 2]
goal = cs[GREEN]
#goal = [(enemy[0]+ball[0])/2,(enemy[1]+ball[1])/2]
else: # the midpoint of the line between the ball and the other bot is the goal
front = cs[BLUE]
back = cs[YELLOW]
start = [(back[0] + front[0]) / 2, (back[1] + front[1]) / 2]
enemy_front = cs[RED]
enemy_back = cs[WHITE]
enemy = [(enemy_back[0] + enemy_front[0]) / 2,
(enemy_back[1] + enemy_front[1]) / 2]
goal = [(enemy[0] + ball[0]) / 2, (enemy[1] + ball[1]) / 2]
#goal = cs[GREEN]
if reset:
goal = startPoints[bot]
if game_over:
goal = [H / 2, W / 2]
if np.isnan(enemy[0]):
enemy[0] = -enemyRadius
if np.isnan(enemy[1]):
enemy[1] = -enemyRadius
if np.isnan(ball[0]) or np.isnan(ball[1]):
ball = lastBall[bot]
if np.isnan(goal[0]) or np.isnan(goal[1]):
# Can't see ball
#print("Can't see ball, going to centre")
goal = lastBall[bot]
canSeeBall = False
else:
lastBall[bot] = goal
if not (np.isnan(start[0]) or np.isnan(start[1])):
goalDisp = [0, 0]
startNode = prm.Node(start[0], start[1], N)
goalNode = prm.Node(goal[0] + goalDisp[0], goal[1] + goalDisp[1],
N + 1)
frontNode = prm.Node(front[0], front[1], N)
enemy = prm.Obstacle(enemy[0], enemy[1], enemyRadius)
ballNode = prm.Obstacle(ball[0], ball[1], ballRadius)
if not (reset and motion.dist(startNode, goalNode) < 70):
#Find path
x, y = prm.pathPlan(graph,
startNode,
goalNode,
enemy,
ballNode,
k,
avoidBall=False,
draw=True,
w=640,
h=480,
frame=frame,
bot=bot)
next = prm.Node(x, y, 0)
'''
if motion.dist(startNode,goalNode)<enemyRadius:
#print("RESET NEXT TO GOAL")
next = prm.Node(goalNode.x,goalNode.y,0)
'''
last[bot] = angle[bot]
lastl[bot] = distance[bot]
if (turnVel[bot] != 0):
#Turn to goal
Il[bot] = 0
turnVel[bot], angle[bot] = motion.PID_Angle(
startNode, frontNode, next, k_angle[bot], I[bot],
last[bot], tol, bot)
#turnVel[bot],angle[bot] = motion.Stochastic_Angle(startNode, frontNode, next, tol, bot)
if np.isnan(turnVel[bot]):
print("ANGLE IS NAN")
turnVel[bot] = 0
limitSet = [0, 0, 0, 0, 0, turnVel[bot]]
client.send_to_server(limitSet, servers[bot],
sockets[bot]) #Sending sets to servers
results = client.get_replies(sockets[bot])
elif velocity[bot] != 0:
#Move to goal
I[bot] = 0
velocity[bot], distance[bot] = motion.PID_Linear(
startNode, next, k_linear[bot], Il[bot], lastl[bot], toll,
bot)
#velocity[bot],distance[bot] = motion.Stochastic_Linear(startNode, next, toll, bot)
if np.isnan(velocity[bot]):
print("VELOCITY IS NAN")
velocity[bot] = 0
limitSet = [velocity[bot], 0, 0, 0, 0, 0]
client.send_to_server(limitSet, servers[bot],
sockets[bot]) #Sending sets to servers
results = client.get_replies(sockets[bot])
else:
#Do nothing
turnVel[bot] = 3
velocity[bot] = 3
#print("turnVel = %i, Velocity = %i" %(turnVel[bot],velocity[bot]))
#limitSet = [0, 0, 0, 0, 0, 0]
I[bot] += angle[bot]
Il[bot] += distance[bot]
if bot == 0 and motion.dist(startNode,
ballNode) < 70 and canSeeBall:
return True
return False
if not (np.isnan(start[0]) or np.isnan(start[1])):
goalDisp = [0, 0]
startNode = prm.Node(start[0], start[1], N)
goalNode = prm.Node(goal[0] + goalDisp[0], goal[1] + goalDisp[1],
N + 1)
frontNode = prm.Node(front[0], front[1], N)
enemy = prm.Obstacle(enemy[0], enemy[1], enemyRadius)
ballNode = prm.Obstacle(ball[0], ball[1], ballRadius)
#Find path
x, y = prm.pathPlan(graph,
startNode,
goalNode,
enemy,
ballNode,
k,
avoidBall=False,
draw=True,
w=640,
h=480,
frame=frame)
next = prm.Node(x, y, 0)
if motion.dist(startNode, goalNode) < 100:
print("RESET NEXT TO GOAL")
next = prm.Node(goalNode.x, goalNode.y, 0)
last = angle
lastl = distance
if (turnVel != 0):
goal = [W / 2, H / 2]
# Set nodes
startNode = prm.Node(back[0], back[1], N)
goalNode = prm.Node(goal[0], goal[1], N + 1)
frontNode = prm.Node(front[0], front[1], N)
# obstacle
enemy = prm.Obstacle(-10, -10, 1)
# first goal
ball = prm.Obstacle(-10, -10, 1)
#Find path
x, y = prm.pathPlan(graph,
startNode,
goalNode,
enemy,
ball,
k,
avoidBall=False,
draw=False)
next = prm.Node(x, y, 0)
# prev_angle = angle
# prev_distance = distance
if iteration % correction_step == 0:
#Reset points to correct any mistakes
turnVel = 3
velocity = 3
if (turnVel != 0):
#Turn to goal
# angular_velocity, angle = motion.PID_Angle(startNode, frontNode, next, k_angle, K_i_angle, prev_angle, tol_angle)
