def goovalaux(perc0):
global perc
perc = perc0
#g.goodmarkers = [25]
driving.drive(0)
time.sleep(4)
sp = 15
driving.drive(sp)
while True:
print("1")
driving.steer(0)
print("2")
nav2.goto_1(1.9, 17)
print("3")
print("marker %s" % (str(g.lastpos)))
driving.steer(-100)
print("4")
# 250 comes from pi*80 (cm)
# it's the outer radius, but so is the speed we get
print("finspeed1 %f dang1 %f ang1 %f" % (g.finspeed, g.dang, g.ang%360))
time.sleep(250.0/g.finspeed*perc)
print("finspeed2 %f dang2 %f ang2 %f" % (g.finspeed, g.dang, g.ang%360))
driving.steer(0)
print("5")
nav2.goto_1(0.5, 13)
print("6")
driving.steer(-100)
time.sleep(250.0/g.finspeed*perc)
def goovalaux(perc0):
global perc
perc = perc0
#g.goodmarkers = [25]
driving.drive(0)
time.sleep(4)
sp = 15
driving.drive(sp)
while True:
print("1")
driving.steer(0)
print("2")
nav2.goto_1(1.9, 17)
print("3")
print("marker %s" % (str(g.lastpos)))
driving.steer(-100)
print("4")
# 250 comes from pi*80 (cm)
# it's the outer radius, but so is the speed we get
print("finspeed1 %f dang1 %f ang1 %f" %
(g.finspeed, g.dang, g.ang % 360))
time.sleep(250.0 / g.finspeed * perc)
print("finspeed2 %f dang2 %f ang2 %f" %
(g.finspeed, g.dang, g.ang % 360))
driving.steer(0)
print("5")
nav2.goto_1(0.5, 13)
print("6")
driving.steer(-100)
time.sleep(250.0 / g.finspeed * perc)
def platoon(other):
driving.drive(0)
while other not in g.otherpos:
time.sleep(1)
print("a queue appeared for %s" % other)
q = g.otherpos[other]
follow = False
lastx = None
laxty = None
slowsp = 15
fastsp = 25
goingslow = True
tb1 = None
while True:
#print("q length %d" % q.qsize())
(x1, y1) = g.posnow[other]
t0 = time.time()
(x, y) = q.get()
t1 = time.time()
#print("got (%f %f) (%f %f)" % (x, y, x1, y1))
q.task_done()
if y1 > 15.0 and not follow:
follow = True
driving.drive(fastsp)
if not follow:
continue
if t1-t0 > 0.1:
print("waited %f" % (t1-t0))
near = 0.5+0.3
near = 0.6+0.3
d = dist(x1, y1, g.ppx, g.ppy)
if d < near and not goingslow:
print("dist %f, speed %d" % (d, slowsp))
goingslow = True
driving.drive(slowsp)
if d > near and goingslow:
print("dist %f, speed %d" % (d, fastsp))
goingslow = False
driving.drive(fastsp)
if lastx != None:
if dist(x, y, lastx, lasty) < 0.5:
continue
lastx = x
lasty = y
tb0 = time.time()
if tb1 != None and tb0-tb1 > 0.1:
print("waitedb %f" % (tb0-tb1))
status = nav2.goto_1(x, y)
#print((status, g.ppx, g.ppy))
tb1 = time.time()
def platoon(other):
driving.drive(0)
while other not in g.otherpos:
time.sleep(1)
print("a queue appeared for %s" % other)
q = g.otherpos[other]
follow = False
lastx = None
laxty = None
slowsp = 15
fastsp = 25
goingslow = True
tb1 = None
while True:
#print("q length %d" % q.qsize())
(x1, y1) = g.posnow[other]
t0 = time.time()
(x, y) = q.get()
t1 = time.time()
#print("got (%f %f) (%f %f)" % (x, y, x1, y1))
q.task_done()
if y1 > 15.0 and not follow:
follow = True
driving.drive(fastsp)
if not follow:
continue
if t1 - t0 > 0.1:
print("waited %f" % (t1 - t0))
near = 0.5 + 0.3
near = 0.6 + 0.3
d = dist(x1, y1, g.ppx, g.ppy)
if d < near and not goingslow:
print("dist %f, speed %d" % (d, slowsp))
goingslow = True
driving.drive(slowsp)
if d > near and goingslow:
print("dist %f, speed %d" % (d, fastsp))
goingslow = False
driving.drive(fastsp)
if lastx != None:
if dist(x, y, lastx, lasty) < 0.5:
continue
lastx = x
lasty = y
tb0 = time.time()
if tb1 != None and tb0 - tb1 > 0.1:
print("waitedb %f" % (tb0 - tb1))
status = nav2.goto_1(x, y)
#print((status, g.ppx, g.ppy))
tb1 = time.time()
def runfile(file):
f = open(file)
driving.drive(20)
for line0 in f:
line = line0[:-1]
l = line.split("\t")
x = float(l[0])
y = float(l[1])
#t = float(l[2])
print((x, y))
t = time.time()
ti = int(t)
if ti/2 % 2 == 0:
driving.drive(20)
else:
driving.drive(10)
status = nav2.goto_1(x, y)
print((status, g.ppx, g.ppy))
driving.drive(0)
def runfile(file):
f = open(file)
driving.drive(20)
for line0 in f:
line = line0[:-1]
l = line.split("\t")
x = float(l[0])
y = float(l[1])
#t = float(l[2])
print((x, y))
t = time.time()
ti = int(t)
if ti / 2 % 2 == 0:
driving.drive(20)
else:
driving.drive(10)
status = nav2.goto_1(x, y)
print((status, g.ppx, g.ppy))
driving.drive(0)
def gopath(path00, plen):
global lastwaypoint
g.last_send = None
path0 = [i for (i, _) in path00]
#out(2, "path00 = %s" % (str(path00)))
outstr = "1 gopath: %d %s" % (path00[0][1], str(path0[0:plen]))
if path00[plen:] != []:
outstr += " + %s" % (str(path0[plen:]))
out(1, outstr)
#out(2, "1 speedsign = %d" % g.speedsign)
g.speedsign = 1
# two lanes:
#path = nav_map.piece2path(path0, -0.25)
# experimental, to make it crash more seldom:
#path = nav_map.piece2path(path0, -0.1)
# single lane:
path = nav_map.piece2path(path0, 0)
boxp = False
if boxp:
# the simulation can't make it without bigger margins
lpath = nav_map.piece2path(path0, -0.15)
rpath = nav_map.piece2path(path0, -0.35)
lx = None
ly = None
rx = None
ry = None
i1 = -1
# for j in range(0, len(path)):
for j in range(0, plen):
(i, x, y) = path[j]
# pretend this is level 2:
out(1, "2 goto %s = %s" % (str(i), str(path00[j])))
tolog("nav1 goto_1 %d %f %f" % (i, x, y))
if i == lastwaypoint:
continue
#pass
if lastwaypoint != None:
nav_tc.send_to_ground_control("between %d %d" % (lastwaypoint, i))
lastwaypoint = i
if g.remote_control:
print("1 remote control/crash")
yield 0
if False:
i2 = i1
i1 = i
if j == len(path) - 1:
i3 = -1
else:
(i3, _, _) = path[j + 1]
nav_tc.send_to_ground_control("between %d %d %d" % (i2, i1, i3))
if boxp:
lxprev = lx
rxprev = rx
lyprev = ly
ryprev = ry
(_, lx, ly) = lpath[j]
(_, rx, ry) = rpath[j]
if lxprev != None:
if False:
print(
"keep between (%f,%f) - (%f,%f) and (%f,%f) - (%f,%f)"
% (lxprev, lyprev, lx, ly, rxprev, ryprev, rx, ry))
g.currentbox = [(lxprev, lyprev, lx, ly),
(rxprev, ryprev, rx, ry)]
if True:
status = nav2.goto_1(x, y)
else:
time.sleep(1)
status = 0
if status != 0:
out(
2, "1 goto_1 returned %d for node %d; we are at (%f, %f)" %
(status, i, g.ppx, g.ppy))
if status == 2:
#driving.drive(-20)
#time.sleep(2)
driving.drive(0)
yield 0
yield 1
return