def main():
ROBOT1 = np.array([[]])
ROBOT2 = np.array([[]])
BALL = np.array([])
ROBOT1 = track.image('two.jpg', 'blue', 'robotb')
ROBOT2 = track.image('two.jpg', 'green', 'robotg')
BALL = track.image('two.jpg', 'red', 'ball')
#print(ROBOT1)
#print(ROBOT2)
#print(BALL)
print("\n POSITION of ROBOTB: (" + str(ROBOT1[4]) + "," + str(ROBOT1[5]) +
") => (" + str(ROBOT1[4] * 30 / 140) + " cm," +
str(ROBOT1[5] * 30 / 144) + " cm)")
print("\n ORIENTATION of ROBOTB: " + str(ROBOT1[6]))
print("\n\n POSITION of ROBOTG: (" + str(ROBOT2[4]) + "," +
str(ROBOT2[5]) + ") => (" + str(ROBOT2[4] * 30 / 140) + "cm ," +
str(ROBOT2[5] * 30 / 144) + " cm)")
print("\n ORIENTATION of ROBOTG: " + str(ROBOT2[6]))
print("\n\n POSITION OF BALL: (" + str(BALL[0]) + "," + str(BALL[1]) +
") => (" + str(BALL[0] * 30 / 140) + "cm ," +
str(BALL[1] * 30 / 144) + " cm)")
print("\n")
def main():
ROBOT1 = np.array([[]])
ROBOT2 = np.array([[]])
BALL = np.array([])
ROBOT1 = track.image('two.jpg','blue','robotb')
ROBOT2 = track.image('two.jpg','green','robotg')
BALL = track.image('two.jpg','red','ball')
#print(ROBOT1)
#print(ROBOT2)
#print(BALL)
print("\n POSITION of ROBOTB: ("+str(ROBOT1[4])+","+str(ROBOT1[5])+") => ("+str(ROBOT1[4]*30/140)+" cm,"+str(ROBOT1[5]*30/144)+" cm)")
print("\n ORIENTATION of ROBOTB: "+str(ROBOT1[6]))
print("\n\n POSITION of ROBOTG: ("+str(ROBOT2[4])+","+str(ROBOT2[5])+") => ("+str(ROBOT2[4]*30/140)+"cm ,"+str(ROBOT2[5]*30/144)+" cm)")
print("\n ORIENTATION of ROBOTG: "+str(ROBOT2[6]))
print("\n\n POSITION OF BALL: ("+str(BALL[0])+","+str(BALL[1])+") => ("+str(BALL[0]*30/140)+"cm ,"+str(BALL[1]*30/144)+" cm)")
print("\n")
def main():
#scaling factors
#sfx = 30.0/140.0
#sfy = 30.0/144.0
#scaling factors
#sfx = 30.0/148.0
#sfy = 30.0/144.0
#y_off = 2/sfy
#ball = np.array([])
#robot1 = np.array([])
#robot2 = np.array([])
'''
#get robot1 position
#(x1,y1,x2,y2,x,y,theta)
robot1 = track.video(1, 'red')
print(robot1)
print(sfx*robot1[0], sfy*robot1[1], sfx*robot1[2], sfy*robot1[3], sfx*robot1[4], sfy*robot1[5], robot1[6])
dx = robot1[2] - robot1[0]
dy = robot1[3] - robot1[1]
d = math.sqrt(pow(dx,2) + pow(dy,2))
da = math.sqrt(pow(sfx*dx,2) + pow(sfy*dy,2))
print(dx, dy, d, sfx*dx, sfy*dy, da)
print('\n')
#get robot2 position
#(x1,y1,x2,y2,x,y,theta)
#robot2 = track.video(1, 'green')
#print(robot2[6])
'''
'''
#get ball's position
#(x,y,w,h)
ball = track.video(1, 'red')
#print(ball)
print(sfx*ball[0], sfy*ball[1], sfx*ball[2], sfy*ball[3])
time.sleep(5)
#get ball's position
#(x,y,w,h)
ball = track.video(1, 'red')
#print(ball)
print(sfx*ball[0], sfy*ball[1], sfx*ball[2], sfy*ball[3])
#robot1 = track.video(1, 'red')
#print(robot1)
#print(sfx*(robot1[2]-robot1[0]))
#to track robot track.video(camera_no, color_name_in_string)
#returns numpy array of position and orientation of robot
'''
ROBOT1 = np.array([[]])
ROBOT2 = np.array([[]])
BALL = np.array([])
ROBOT1 = track.image('btt.jpg','blue','robotb')
ROBOT2 = track.image('btt.jpg','green','robotg')
BALL = track.image('btt.jpg','red','ball')
print("\n POSITION of ROBOTB: ("+str(ROBOT1[4])+","+str(ROBOT1[5])+")")
print("\n ORIENTATION of ROBOTB: "+str(ROBOT1[6]))
print("\n\n POSITION of ROBOTG: ("+str(ROBOT2[4])+","+str(ROBOT2[5])+")")
print("\n ORIENTATION of ROBOTG: "+str(ROBOT2[6]))
print("\n\n POSITION OF BALL: ("+str(BALL[0])+","+str(BALL[1])+")\n")
def main():
#scaling factors
#sfx = 30.0/140.0
#sfy = 30.0/144.0
#scaling factors
#sfx = 30.0/148.0
#sfy = 30.0/144.0
#y_off = 2/sfy
#ball = np.array([])
#robot1 = np.array([])
#robot2 = np.array([])
'''
#get robot1 position
#(x1,y1,x2,y2,x,y,theta)
robot1 = track.video(1, 'red')
print(robot1)
print(sfx*robot1[0], sfy*robot1[1], sfx*robot1[2], sfy*robot1[3], sfx*robot1[4], sfy*robot1[5], robot1[6])
dx = robot1[2] - robot1[0]
dy = robot1[3] - robot1[1]
d = math.sqrt(pow(dx,2) + pow(dy,2))
da = math.sqrt(pow(sfx*dx,2) + pow(sfy*dy,2))
print(dx, dy, d, sfx*dx, sfy*dy, da)
print('\n')
#get robot2 position
#(x1,y1,x2,y2,x,y,theta)
#robot2 = track.video(1, 'green')
#print(robot2[6])
'''
'''
#get ball's position
#(x,y,w,h)
ball = track.video(1, 'red')
#print(ball)
print(sfx*ball[0], sfy*ball[1], sfx*ball[2], sfy*ball[3])
time.sleep(5)
#get ball's position
#(x,y,w,h)
ball = track.video(1, 'red')
#print(ball)
print(sfx*ball[0], sfy*ball[1], sfx*ball[2], sfy*ball[3])
#robot1 = track.video(1, 'red')
#print(robot1)
#print(sfx*(robot1[2]-robot1[0]))
#to track robot track.video(camera_no, color_name_in_string)
#returns numpy array of position and orientation of robot
'''
ROBOT1 = np.array([[]])
ROBOT2 = np.array([[]])
BALL = np.array([])
ROBOT1 = track.image('btt.jpg', 'blue', 'robotb')
ROBOT2 = track.image('btt.jpg', 'green', 'robotg')
BALL = track.image('btt.jpg', 'red', 'ball')
print("\n POSITION of ROBOTB: (" + str(ROBOT1[4]) + "," + str(ROBOT1[5]) +
")")
print("\n ORIENTATION of ROBOTB: " + str(ROBOT1[6]))
print("\n\n POSITION of ROBOTG: (" + str(ROBOT2[4]) + "," +
str(ROBOT2[5]) + ")")
print("\n ORIENTATION of ROBOTG: " + str(ROBOT2[6]))
print("\n\n POSITION OF BALL: (" + str(BALL[0]) + "," + str(BALL[1]) +
")\n")
