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 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 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()
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 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