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