def endGameSequence(centerPos1, centerPos2, rotationRobot1, rotationRobot2, distance, buffer, speed, displaywidth, displayheight):
distance1 = distance
distance2 = distance
futureForward1 = [centerPos1[0] + speed * math.cos(rotationRobot1), centerPos1[1] + speed * math.sin(rotationRobot1)]
futureBackward1 = [centerPos1[0] - speed * math.cos(rotationRobot1), centerPos1[1] - speed * math.sin(rotationRobot1)]
if not Line_Functions.isHittingAnyBoundary(futureForward1, distance, 30, displaywidth, displayheight) and not Line_Functions.isIntersecting(futureBackward1, centerPos2, distance, distance, 20):
print "Move Forward 1"
startBounce(0, 'f', 0.5)
elif not Line_Functions.isHittingAnyBoundary(futureBackward1, distance1, 30, displaywidth, displayheight)and not Line_Functions.isIntersecting(futureBackward1, centerPos2, distance1, distance2, 20):
print "Move Backward 1"
startBounce(0, 'b', 0.5)
else:
print "Stop 2"
stop(1)
futureForward2 = [centerPos2[0] + speed * math.cos(rotationRobot2), centerPos2[1] + speed * math.sin(rotationRobot2)]
futureBackward2 = [centerPos2[0] - speed * math.cos(rotationRobot2), centerPos2[1] - speed * math.sin(rotationRobot2)]
if not Line_Functions.isHittingAnyBoundary(futureForward2, distance2, 30, displaywidth, displayheight)and not Line_Functions.isIntersecting(centerPos1, futureForward2, distance1, distance2, 20):
print "Move Forward 2"
startBounce(1, 'f', 0.5)
elif not Line_Functions.isHittingAnyBoundary(futureBackward2, distance2, 30, displaywidth, displayheight)and not Line_Functions.isIntersecting(centerPos1, futureBackward2, distance1, distance2, 20):
print "Move Backward 2"
startBounce(1, 'b', 0.5)
else:
print "Stop 2"
stop(1)
# --- Game logic
#
# The method will be sending a tuple of the following form, where each point is expressed as(x,y) where x and y are between 0 and 1:
#
# ([center-point-front-robot-1, center-point-rear-robot-1],
# [center-point-front-robot-2, center-point-rear-robot-2])
#
#Test Data One
testData = [[(0.25, 0.25), (0.3, 0.3)], [(0.7, 0.7), (0.7, 0.8)]]
#Made testData below to test out bullets
testData2 = [[(0.5, 0.7), (0.5,0.8)], [(0.7, 0.7), (0.7, 0.8)]]
testData = Line_Functions.convertVisionDataToScreenCoords(testData, displaywidth, displayheight)
#testData = [[(Robot 1)(x1, y1), (x2, y2)], [(Robot 2)(x1, y1), (x2, y2)]]
#Split Test Data into two robots
robot1 = testData[0] #[(0.25, 0.25), (0.3,0.3)]
robot2 = testData[1] #[(0.7, 0.7), (0.7, 0.8)]
#Find centers
centerRobot1 = Line_Functions.centerOfLine(robot1[0], robot1[1])
centerRobot2 = Line_Functions.centerOfLine(robot2[0], robot2[1])
x1 = int(centerRobot1[0]); y1 = int(centerRobot1[1]); x2 = int(centerRobot2[0]); y2 = int(centerRobot2[1])
#Determine the distence between the two given points
distance1 = int(60)
"""baddata = False
if(xS<=0 or yS<=0 or xB<=0 or yB<=0 or xS>=1 or yS>=1 or xB>=1 or yB>=1 ):
baddata = True
if(x1S<=0 or y1S<=0 or x1B<=0 or y1B<=0 or x1S>=1 or y1S>=1 or x1B>=1 or y1B>=1):
baddata = True
if baddata:
centerRobot1 = lastPosRobot1
rotationRobot1 = lastRotationRobot1
centerRobot2 = lastPosRobot2
rotationRobot2 = lastRotationRobot2
"""
# else:
testData = [[(xS, yS), (xB, yB)], [(x1S, y1S), (x1B, y1B)]]
testData = Line_Functions.convertVisionDataToScreenCoords(testData, displaywidth, displayheight)
# Split Test Data into two robots
robot2 = testData[0]
robot1 = testData[1]
# Find centers
centerRobot1 = Line_Functions.centerOfLine(robot1[0], robot1[1])
centerRobot2 = Line_Functions.centerOfLine(robot2[0], robot2[1])
# Determine the degrees the robot is facing from east
rotationRobot1 = Line_Functions.rotationOfLine(robot1[0], robot1[1])
rotationRobot2 = Line_Functions.rotationOfLine(robot2[0], robot2[1])
"""###Used to look for bad data from vision, if bad, set the pos/rotation to the last good data received
if lastPosRobot1 is not None:
