def goToCube(n,timeout=45):
t0 = R.time()
res = False
for p in prepositions[n]:
res = route.goToPointStraight(None,p)
if res > 0:
print("goToCube failed!")
route.goToPointStraight(None,
translateToZone(Position(1500,1500)))
return False
drive.driveRotateToAngle(preangles[n],anglehint[n])
mks = R.see()
p = findPosition(mks)
if p is None:
p = (prepositions[n][-1],anglehint[n])
pre,c = None,None
for mk in mks:
if mk.info.code == cube.getNthCode(n):
c = Cube(mk,p[0].x,p[0].y,p[1])
pre = orienting.getPre(p[0],c.p,c.a)
break
if pre is None or c is None:
return False
presim = conversions.toSimCoords(pre)
print(f"Cube: {c} Pre: {presim}")
return True
def test6():
p = Position(500, 500)
markers = R.see()
A.addMarkers(markers)
print(A)
pts = [Position(1950, 1950), Position(1850, 1850)]
for pt in pts:
print(conversions.toSimCoords(pt), A.ptClear(pt, 80))
print("-----------")
prList(A.getRoutePts(p))
prList(A.getRoutePts(p, Position(1000, 1000)))
def __init__(self,*args,**kwargs):
if 'code' in kwargs:
self.code,self.color = kwargs['code'],kwargs['color']
self.p,self.a = kwargs['p'],kwargs['a']
self.x,self.y,self.ts = self.p.x,self.p.y,R.time()
return
elif len(args) >= 4:
marker = args[0]
rx = args[1]
ry = args[2]
ra = args[3]
else:
raise TypeError
#ra is the robot's angle, measured anticlockwise from x-axis
#ra - marker.rot_y is the angle between the x-axis and the
#line between the cube and the robot
#ra - marker.orientation.rot_y is the anticlockwise angle
#between the negative x-axis and the normal to the marker
#self.a is the anticlockwise angle between the marker's normal
#and the positive x-axis
#print("\n\n--------------------")
d = marker.dist * 1000
orient = position.wtf2(marker.rot_y,marker.orientation.rot_y)
#print(f"Marker distance {d} orientation {marker.orientation.rot_y}")
self.a = (ra - orient + 180) % 360
#print(f"Marker angle {self.a}")
adiff = ra - marker.rot_y
#print(f"Adiff {adiff}")
self.x = rx + deg.sin(adiff) * d
self.y = ry + deg.cos(adiff) * d
p = conversions.toSimCoords(Position(self.x,self.y))
#print("Before correction: {0} {1}".format(p,self.a))
#apply corrections to x and y due to size of cube
self.x -= deg.sin(self.a) * 180
self.y -= deg.cos(self.a) * 180
self.__updateP()
#print("After correction:",conversions.toSimCoords(self.p))
self.color = marker.info.marker_type
self.code = marker.info.code
self.ts = R.time()
def prList(l):
for i in l:
print(conversions.toSimCoords(i))
def prList(l):
for i in l:
print(conversions.toSimCoords(i))
def test6():
p = Position(500, 500)
markers = R.see()
A.addMarkers(markers)
print(A)
pts = [Position(1950, 1950), Position(1850, 1850)]
for pt in pts:
print(conversions.toSimCoords(pt), A.ptClear(pt, 80))
print("-----------")
prList(A.getRoutePts(p))
prList(A.getRoutePts(p, Position(1000, 1000)))
while True:
m = R.see()
#A.addMarkers(m)
cp = position.findPosition(m)
if cp is None:
continue
print("Current position: {0} {1}".format(conversions.toSimCoords(cp[0]),
cp[1]))
print("Arena:\n", A)
print("Closest: ", A.getNearest(cp[0], MARKER_TOKEN_SILVER))
R.sleep(1)
def __str__(self):
return "p: {0} code: {1} col: {2}".format(conversions.toSimCoords(self.p),self.code,self.color)