import colour
from colour import RED, WHITE, ORANGE, YELLOW, GREEN, BLUE
import prm
import motion
import client
if __name__ == "__main__":
N = 9 #Number of samples for PRM
k = 10 #Number of nearest neighbours for PRM
W = 640 #Width of arena
H = 480 #Height of arena
Pad = 50 #Padding around edges
manual = True
graph = prm.initGraph(N, H, W, Pad, k, manual)
cap = cv2.VideoCapture(0)
client.createConnections() #Creates three connections to servers
#PID Stuff
k_angle = [0.15, 0.025, 0.1]
k_linear = [0.002, 0.0001, 0.005]
I = 0
Il = 0
last = 0
lastl = 0
angle = 0
distance = 0
tol = 0.25
toll = 0.01
turnVel = 3
N = 100
k = 10
wallPad = 100
h = 640
w = 480
directory = "footage/"
format = ".avi"
timestamp = time.ctime(time.time()).split(" ")[4].split(":")
filename = "output-"+timestamp[0]+"-"+timestamp[1]+"-"+timestamp[2]
print(timestamp)
fourcc = cv2.VideoWriter_fourcc('M','J','P','G')
out = cv2.VideoWriter(directory+filename+format, fourcc, 20.0, (int(cap.get(3)),int(cap.get(4))))
graph = prm.initGraph(N,w,h,wallPad,k)
start = prm.Node(np.random.randint(0,w-wallPad),np.random.randint(0,h-wallPad),N)
goal = prm.Node(w/2,h/2,N+1)
enemy = prm.Obstacle(w/2,h/2,60)
ball = prm.Obstacle(np.random.randint(0,w-wallPad),np.random.randint(0,h-wallPad),10)
font = cv2.FONT_HERSHEY_SIMPLEX
topLeft = (20,50)
fontScale = 1
fontColor = (255,255,255)
lineType = 2
while(True):
# Capture frame-by-frame
# server names for turtlebots which we connect to
servers = ["10.199.61.21", "10.199.26.14"]
# ports in which the messages pass through
ports = [12007, 12006]
# sockets for each port/server connection
sockets = [0, 1]
if __name__ == "__main__":
N = 100 # Number of samples for PRM
k = 10 # Number of nearest neighbours for PRM
W = 640 # Width of arena
H = 480 # Height of arena
Pad = 80 # Padding around edges
graph = prm.initGraph(N, W, H, Pad, k)
cap = cv2.VideoCapture(0)
# turtlebot server 1
client.create_connections(servers[0], ports[0], sockets[0])
# turtlebot server 2
# client.create_connections(servers[1], ports[1], sockets[1])
# PID Stuff
prev_angle = 0
prev_distance = 0
# kp, ki, kd
k_angle = [0.15, 0.005, 0]
k_distance = [0.001, 0.00005, 0]
# integral control
K_i_angle = 0