def approachCube(color=MARKER_TOKEN_GOLD, timeout=5):
min_dist = 80
t0 = R.time()
if not checkColor(color):
return False
i = 1
while R.time() < t0 + timeout:
i += 1
leftd = ultrasound.getDistance(0)
rightd = ultrasound.getDistance(1)
print(f"Left {leftd} Right {rightd}")
m = None
if leftd is None and rightd is None:
print("Straight")
drive.driveStraight(20)
elif leftd is None:
print("Rotating CW")
drive.drive(20, 10, -1)
m = rightd
elif rightd is None:
print("Rotating CCW")
drive.drive(10, 20, -1)
m = leftd
else:
print("Straight slowly")
drive.driveStraight(15)
m = leftd
if not m is None and m < min_dist:
print("Stopping")
drive.driveStraight(0)
return True
return False
def lineUpToCubeCam(code, timeout=5, t0=R.time()):
while R.time() < t0 + timeout:
markers = R.see()
for m in markers:
if m.info.code == code:
mk = m
break
if mk is None:
return False
def getRoutePts(self,p,target=None,lim=5,tlim=15):
#tlim is the time since seeing the cube
global robot_cube_distance
pts = []
platform_margin = 160
for cube in self.cubeList:
if cube.p.onPlatform():
continue
if R.time() - cube.ts <= tlim:
pts += cube.getRoutePts(p,robot_cube_distance+50)
poss = (5750/2-600-platform_margin,5750/2+600+platform_margin)
for x in poss:
for y in poss:
pts.append(Position(x,y))
if not target is None:
pts = sortPts(pts,p,target)
i = 0
ret = []
for pt in pts:
if len(ret) >= lim:
break
if self.pathClear(p,pt):
ret.append(pt)
#else:
#print("Point {0} isn't clear".format(pt))
print("Potential route points:",ret)
return ret
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 nthCubePositionCorrect(n,pos):
global codes
mks = R.see()
code = getNthCode(n)
for m in mks:
if m.info.marker_type == MARKER_ARENA:
continue
c = Cube(m)
if c.code == code:
if c.p.dist(pos) < 120:
return 1,c.p.dist(pos)
return -1,c.p.dist(pos)
return 0,0
def approachCubeCam(code, timeout=10):
min_dist = 140
s_per_deg = 0.2
mk = None
print("approachCubeCam")
t0 = R.time()
while R.time() < t0 + timeout:
markers = R.see()
for m in markers:
if m.info.code == code:
mk = m
break
if mk is None:
return False
print("Dist:", mk.dist * 1000)
left = ultrasound.getDistance(0)
right = ultrasound.getDistance(1)
print("US", left, right)
print("Switch", R.ruggeduinos[0].digital_read(2))
if not left is None and not right is None:
if min(left, right) < 60 or mk.dist < 0.145 or R.ruggeduinos[
0].digital_read(2):
#R.sleep(0.5)
drive.driveStraight(0)
return True
if mk.rot_y > -0.5 and mk.rot_y < 0.5:
print("Straight")
drive.driveStraight(20)
elif mk.rot_y > 0:
print("Right")
drive.drive(15, 10, -1)
elif mk.rot_y < 0:
print("Left")
drive.drive(10, 15, -1)
print("Timed out!")
return False
def getNearest(self,p,col,t=6,n=0,inZone=False):
#col=colour, t=time since seeing cube, n=nth furthest (0=closest)
l = []
for ind in range(len(self.cubeList)):
cube = self.cubeList[ind]
if cube.color != col:
continue
if R.time() - cube.ts > t:
continue
if cubeInZone(cube,R.zone) != inZone:
continue
l.append(self.cubeList[ind])
l.sort(key=lambda c: c.p.dist(p))
if len(l) <= n:
return None
return l[n]
def checkColor(color):
markers = R.see()
markers.sort(key=lambda m: m.dist)
if len(markers) == 0:
return True #assume it is correct if no markers are visible
if markers[0].info.marker_type != color:
print("Ugh! A silver! I didn't ask for a _silver_!!!")
print(markers[0].info.code)
return False
"""
for m in markers:
if m.dist * 1000 < 300 and not color is None:
if m.info.marker_type == MARKER_ARENA:
continue
if math.fabs(m.rot_y) > 20:
continue
if m.info.marker_type != color:
print(f"Exiting {m.dist} {m.info.marker_type}")
return False
"""
return True
def goToPointStraight(prev, nex, timeout=15, p=drive_power):
lastPt = prev
init = prev
t0 = R.time()
pos_ts = R.time()
to_cnt = 0
while True:
if R.time() > t0 + timeout:
print("Timeout1!")
driveStraightSync(-30, 2)
return 1
m = R.see()
cp = position.findPosition(m)
if cp is None:
if R.time() > pos_ts + 3:
print("Timeout2!")
if to_cnt < 3:
driveStraightSync(-30, 2)
else:
driveStraightSync(30, 2)
to_cnt = 0
pos_ts = R.time()
t0 += 4
to_cnt += 1
continue
if init is None:
init = cp[0]
if arrivedPt(cp[0], init, nex):
driveStraight(0)
return 0
if cp[0].dist(nex) > 500 or lastPt is None:
lastPt = cp[0]
if lastPt is None:
continue
checkAngleSync(cp[1], lastPt, nex, p)
pos_ts = R.time()
p2.x = 0
p2.y = 0
"""
while True:
markers = R.see()
#use the arena markers to calculate robot's x,y,angle
x = position.findPosition(markers)
print("Position: {0}".format(conversions.toSimCoords(x[0])))
robot_x,robot_y,robot_a = x[0].x,x[0].y,x[1]
for m in markers:
if m.info.marker_type == MARKER_ARENA:# and m.info.code==6:
#print("Arena marker: ",m.info.code,m.orientation.rot_y)
continue
#make cube, print cube
#c = Cube(m,robot_x,robot_y,robot_a)
#print(c.getRoutePts(Position(0,0),50))
#p = conversions.toSimCoords(Position(c.x,c.y))
#print("Code: {0} Position: {1} Angle: {2}".format(m.info.code,p,c.a))
R.sleep(1)
"""
while True:
print(getPresentCubes())
R.sleep(1)
import orienting
from robot_obj import R
import position
from position import Position
import claw,lift
while True:
lift.raiseLift()
claw.openClawSync()
lift.waitOnLift()
orienting.approachCubeCam(38,20)
print("Waiting...")
R.sleep(3)
print("Done waiting")
def timestamp(s):
print(s + " at {0}".format(R.time()))