def auto(): # I'd like to light LEDs here, but maybe the LEDlight plugin # hasn't started yet. driving.drive(0) while not g.ground_control: time.sleep(3) driving.steer(70) time.sleep(0.5) driving.steer(-70) m3(0.4) while True: ang = g.ang%360 if ang > 180: ang -= 360 print("pos %f %f %f" % (g.ppx, g.ppy, ang)) if (abs(g.ppx-2.5) < 0.5 and abs(g.ppy-14.2) < 0.5 and abs(ang) < 30): break time.sleep(1) driving.steer(0) m1() nav1.whole4() while True: time.sleep(100)
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 connect_to_ecm(): stopped = False s = nav_comm.open_socket2() ecmt0 = time.time() counter = 0 g.crashacc = None while True: if g.crashacc: #g.crash = False if not stopped: counter = 9 #s.send("crash".encode('ascii')) print("crash: %f" % g.crash) g.remote_control = True #stop() driving.drive(0) #speak("ouch") nav_signal.warningblink(True) stopped = True s112 = nav_comm.open_socket3() sstr = ('accident %f %f %f %s\n' % ( g.ppx, g.ppy, g.crashacc, g.VIN)).encode("ascii") print(sstr) s112.send(sstr) print(sstr) resp = s112.recv(1024) print(resp) resp = resp.decode("ascii") print(resp) if True: resp = json.loads(resp) print(resp) say = resp['speak'] else: say = resp print(say) speak(say) s112.close() if True: t = time.time() if t-ecmt0 > 1.0: #s.send("bar".encode('ascii')) g.crashacc = 0.0 if g.crash: g.crashacc = g.crash nav_tc.send_to_ground_control( "message crash %f" % g.crashacc) s.send(('{"crash":%d, "x":%f, "y":%f, "crashacc":%f}\n' % ( counter, g.ppx, g.ppy, g.crashacc)).encode("ascii")) ecmt0 = t time.sleep(0.05) if counter != 9: counter = (counter+1)%8
def connect_to_ecm(): stopped = False s = nav_comm.open_socket2() ecmt0 = time.time() counter = 0 g.crashacc = None while True: if g.crashacc: #g.crash = False if not stopped: counter = 9 #s.send("crash".encode('ascii')) print("crash: %f" % g.crash) g.remote_control = True #stop() driving.drive(0) #speak("ouch") nav_signal.warningblink(True) stopped = True s112 = nav_comm.open_socket3() sstr = ('accident %f %f %f %s\n' % (g.ppx, g.ppy, g.crashacc, g.VIN)).encode("ascii") print(sstr) s112.send(sstr) print(sstr) resp = s112.recv(1024) print(resp) resp = resp.decode("ascii") print(resp) if True: resp = json.loads(resp) print(resp) say = resp['speak'] else: say = resp print(say) speak(say) s112.close() if True: t = time.time() if t - ecmt0 > 1.0: #s.send("bar".encode('ascii')) g.crashacc = 0.0 if g.crash: g.crashacc = g.crash nav_tc.send_to_ground_control("message crash %f" % g.crashacc) s.send(('{"crash":%d, "x":%f, "y":%f, "crashacc":%f}\n' % (counter, g.ppx, g.ppy, g.crashacc)).encode("ascii")) ecmt0 = t time.sleep(0.05) if counter != 9: counter = (counter + 1) % 8
def electric_braking(): ''' Electric brake ''' times = 0 while times < 10: drive(-100) times += 1 drive(0)
def stop1(): driving.drive(0) time.sleep(3) print((g.ppx, g.ppy, g.ang%360)) g.goodmarkers=[(7, 'all', 0.6), (25, 'all', 0.6), (22, 'all', 0.65), (2, 'all', 0.45)] time.sleep(3) print((g.ppx, g.ppy, g.ang%360)) g.goodmarkers=[] driving.drive(20)
def electric_braking(): ''' The most effective brake. Should only be used for safety or in case of very high speeds. ''' amount = 10 while amount > 0: drive(-100) amount -= 1
def brake(ideal_distance): ''' Normal brake should be enough. Simply setting the speed to 0 gives a braking distance of 1 cm/(cm/s). So it should definitely be good enough if we set the ideal distance high enough. ''' distance = g.can_ultra prev_distance = distance while prev_distance - distance > 0.01 or distance < ideal_distance: prev_distance = distance drive(0) time.sleep(0.1) distance = g.can_ultra
def distance_based_brake(set_d): ''' Brakes in small increments to avoid stalling, but at the same time to be very effective ''' distance = g.can_ultra if distance < set_d: speed = g.outspeedcm / 2 while speed > 0: speed -= 5 if speed < 0: speed = 0 drive(speed)
def keep_distance(ideal_distance): ''' Adjusts speed to keep distance ''' prev_distance = g.can_ultra time.sleep(0.1) distance = g.can_ultra #risk for stalling, most likely needs to be heavily adjusted while prev_distance < distance or distance > ideal_distance: drive(19) #just a random chosen value from "the list" prev_distance = distance time.sleep(0.05) distance = g.can_ultra
def activate_acc(set_d): ''' Simple ACC that aims that takes desired distance as parameter ''' while True: distance = g.can_ultra speed = 0 while distance > set_d: speed += 1 drive(speed) while distance < set_d: speed -= 1 drive(speed)
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 follow(): speeds = [0, 7, 11, 15, 19, 23, 27, 37, 41, 45, 49, # 93 to 100 haven't been run yet 53, 57, 73, 77, 81, 85, 89, 93, 97, 100] sp = None while True: oldsp = sp x = (g.finspeed-10)/2 sp = x-10 if sp < 0: sp = 0 print("%f %f" % (g.finspeed, sp)) if sp > 25: sp = 0 if sp != oldsp: driving.drive(sp) time.sleep(0.5)
def acc_run_test(use_electric): ''' Handles braking and logic for ACC ''' while True: prev_distance = g.can_ultra time.sleep(0.25) #Supposedly only gets a value four times per second distance = g.can_ultra relative_speed = calc_relative_speed(prev_distance, distance, 0.25) print(relative_speed) #check is distance has changed over several or one seconds #Calculate relative speed over a longer interval if relative_speed >= 0 or distance > 1: #increase speed, but for now only set to 19 drive(19) else: if use_electric: electric_braking() else: drive(0)
def acc_speed(speed, set_d): ''' Small incremental ACC that works for very small speeds or with frequent data ''' distance = g.can_ultra current_speed = g.outspeedcm / 2 while True: while distance > set_d and current_speed < speed: increase_speed(current_speed) current_speed = g.outspeedcm / 2 distance = g.can_ultra while distance < set_d: electric_braking() drive(0) distance = g.can_ultra current_speed = g.outspeedcm / 2 while distance > set_d and current_speed > speed: decrease_speed(current_speed) current_speed = g.outspeedcm / 2 distance = g.can_ultra
def travel(n0, n1, n2=None, nz=None): routes_p = nav_map.paths_p(n0, n1, n2, nz) if routes_p == []: print("no route found") return False # Value judgment: pick the shortest routes_p.sort() (d1, r1) = routes_p[0] r2 = nav_map.insert_waypoints_l(r1) print((d1, r2)) print("travel1") driving.drive(20) print("travel2") # we should use gopath as a generator gopath(r) print("travel3") driving.drive(0) print("travel4") return True
def act_acc(set_d, brake, desired_speed): ''' Working implementation Our main ACC function set_d: minimum distance to the preceding MOPED desired, in meters brake: takes input 0, 1, 2 for different brake methods used. Defaults to 0 if faulty input is given ''' if brake != (0 or 1 or 2): brake = 0 print(brake) #set true if the car has stopped stopped = True while True: distance = g.can_ultra print(distance) #as long as distance is less than desired distance, keep speed while distance > set_d: speed = desired_speed drive(speed) distance = g.can_ultra #With the new distance, yet again check if the distance is larger than desired distance. #Set stopped to False as the car is ready to drive if distance > set_d: stopped = False print(distance) #if distance is smaller than desired distance, brake. #we have several different brake functions, #depending on whether aggressive braking is needed or not #for aggressive braking electric brake is used, for less aggressive #brake use decremental brake. #distance based brake aims to keep the set_d to the car in front #Electric brake runs the risk of stalling and reversing. #Therefore using it only allows for braking #when distance is larger than 10 cm. while 0 < distance < set_d: distance = g.can_ultra if brake == 0: if not stopped: electric_braking() print(distance) if distance < 0.05: stopped = True drive(0) elif brake == 1: distance_based_brake(set_d) elif brake == 2: decremental_brake(speed) else: drive(0) print(distance)
def electric_brake(ideal_distance): ''' If the MOPED approaches the preceding MOPED by more than 1 cm per every tenth of a second, the car brakes. Also the distance to the preceding car has to be less than the ideal distance. Otherwise there is no reason to brake. ''' drive(0) prev_distance = g.can_ultra time.sleep(0.1) distance = g.can_ultra while prev_distance - distance > 0.01 and distance < ideal_distance: drive(-100) prev_distance = distance drive(0) time.sleep(0.1) distance = g.can_ultra #If distance isn't decreasing, we wait until the preceding car is accelerating #and to avoid that the car crashes while distance < ideal_distance: 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 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 gotoaux(x, y, state): print("gotoaux %f %f %s" % (x, y, state)) driving.drive(0) if state == "accident": g.signalling = True start_new_thread(signal, ()) time.sleep(4) driving.drive(30) status = nav2.goto_1(x, y) if status != 0: print("goto_1 returned %d for (%f, %f); we are at (%f, %f)" % (status, x, y, g.ppx, g.ppy)) return False g.signalling = False driving.drive(0) return True
def gotoaux(x, y, state): print("gotoaux %f %f %s" % (x, y, state)) driving.drive(0) if state == "accident": g.signalling = True start_new_thread(signal, ()) time.sleep(4) driving.drive(30) status = nav2.goto_1(x, y) if status != 0: print("goto_1 returned %d for (%f, %f); we are at (%f, %f)" % ( status, x, y, g.ppx, g.ppy)) return False g.signalling = False driving.drive(0) return True
import driving from windows import Window if __name__ == '__main__': window = Window('Grand Theft Auto V', 310, 26, 2) driving.drive(window, 'xception.h5', 5)
def whole4aux(path0): global thengoal0 qfromplanner = queue.Queue(2) qtoplanner = queue.Queue(2) start_new_thread(planner0, (qfromplanner, qtoplanner)) qtoplanner.put(path0) if g.simulate: speed0 = 30 else: speed0 = 20 driving.drive(0) if not g.simulate: # can be no sleep at all if we already did drive(0) earlier, # but must maybe be 4 if we didn't. pass #time.sleep(4) driving.drive(speed0) g.last_send = None print("speedsign = %d" % g.speedsign) g.speedsign = 1 qfromlower = queue.Queue(2) qtolower = queue.Queue(2) start_new_thread(executor1, (qtolower, qfromlower)) nextplan = None thengoal0 = None while True: if nextplan == None: nextplan = qfromplanner.get() qfromplanner.task_done() p = nextplan nextplan = None thengoal0 = None if p == 'stop': out(1, "0 executor got stop") driving.drive(0) return for status in executor0(p, qtolower, qfromlower): if status == 0: out(0, "0 executor failed, whole4aux exits") driving.drive(0) return else: out(1, "0 executor0 reported %d" % (status)) if not qfromplanner.empty(): if nextplan == None: nextplan = qfromplanner.get() qfromplanner.task_done() out( 2, "1 next plan for executor0 is %s" % (str(nextplan))) if nextplan != 'stop': thengoal0 = nextplan[1] out(1, "0 extending with %s" % str(thengoal0)) # ugly hack, for testing else: out( 2, "0 another new plan for executor0 exists, but we already have one: %s" % str(nextplan))
def planner0(qfromplanner, qtoplanner): # idea: from the current position, determine which piece we can # start with lx = None ly = None rx = None ry = None i1 = -1 path0 = qtoplanner.get() qtoplanner.task_done() out(1, "0 planner got %s" % path0) nextpiece = path0 while True: i10 = nextpiece[-1] i2 = nextpiece[-2] thispiece = nextpiece #print("(%d, %d)" % (i10, i2)) if (i10, i2) == (23, 34): # not possible: 35 #nextpiece = randsel([23, 6], [23, 5]) nextpiece = [23, 5] elif (i10, i2) == (6, 23): # not possible: 5 nextpiece = [6, 35] elif (i10, i2) == (35, 6): nextpiece = randsel([35, 23], [35, 34, 5]) #nextpiece = [35, 34, 5] elif (i10, i2) == (23, 35): # not possible: 34 #nextpiece = randsel([23, 5], [23, 6]) nextpiece = [23, 6] elif (i10, i2) == (5, 23): # not possible: 6 nextpiece = [5, 34] elif (i10, i2) == (34, 5): nextpiece = randsel([34, 23], [34, 35, 6]) # (don't turn: only allow for one kind of give-way situation) #nextpiece = [34, 35, 6] elif (i10, i2) == (23, 6): # not possible: 5 #nextpiece = randsel([23, 35], [23, 34]) nextpiece = [23, 35] elif (i10, i2) == (23, 5): # not possible: 6 # temporarily avoid going 16-23-27 (now named 5-23-35) #nextpiece = randsel([23, 34], [23, 35]) nextpiece = [23, 34] elif (i10, i2) == (5, 34): nextpiece = randsel([5, 6, 35], [5, 23]) elif (i10, i2) == (6, 35): nextpiece = randsel([6, 5, 34], [6, 23]) #nextpiece = [6, 5, 34] elif (i10, i2) == (35, 23): # not possible: 34 nextpiece = [35, 6] elif (i10, i2) == (34, 23): # not possible: 35 nextpiece = [34, 5] else: print("impossible combination (%d, %d), whole4aux exits" % (i10, i2)) driving.drive(0) return #print("nextpiece %s" % str(nextpiece)) #print("thispiece %s" % str(thispiece)) sendplan(qfromplanner, thispiece)
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
def increase_speed(current_speed): ''' Increases speed with a unit of one ''' speed = current_speed + 1 drive(speed)
def decrease_speed(current_speed): ''' Decreases speed with a unit of one ''' speed = current_speed - 1 drive(speed)
def decremental_brake(speed): ''' Brakes in small increments to avoid stalling ''' for number in range(0, speed): drive(speed - (number + 1))
def goto_1(x, y): g.targetx = x g.targety = y missed = False inc = 0 inc2 = 0 lastdist = None brake_s = 0.0 tolog("goto_1 %f %f" % (x, y)) while True: if g.remote_control: print("remote_control is true") return 2 if not checkpos(): if False: print("checkpos returned False") return 2 if g.poserror: if False: print("positioning system error") tolog("poserr 1") g.limitspeed = 0 time.sleep(10) tolog("poserr 2") g.limitspeed = None print("continuing") g.poserror = False tolog("poserr 3") return 2 if g.obstacle: print("obstacle") return 2 dist = getdist(x, y) if g.inspeed != 0: # Assume we are going in the direction of the target. # At low speeds, braking time is about 1.5 s. brake_s = 1.5 * abs(g.inspeed) / 100 # say that braking distance is 1 dm at higher speed, when # braking electrically if g.inspeed > 0: brake_s = 0.4 else: brake_s = 0.6 # we should only use brake_s when we are going to stop brake_s = 0.0 # 'lastdist' is never non-None now, nor 'missed' if lastdist != None: if dist < lastdist - 0.01: inc = -1 lastdist = dist elif dist > lastdist + 0.01: if inc == -1: missed = True tolog("missed target") inc = 1 lastdist = dist tolog("gotoa1 %f %f -> %f %f" % (g.ppx, g.ppy, x, y)) a = atan2(y - g.ppy, x - g.ppx) adeg = 180 / pi * a adeg = 90 - adeg adiff = g.ang - adeg adiff = adiff % 360 tolog("gotoa2 a %f adeg %f adiff %f" % (a, adeg, adiff)) if g.speedsign < 0: adiff += 180 if adiff > 180: adiff -= 360 adiff = -adiff # now, positive means the target is to the right of us tolog("gotoa3 adiff %f" % (adiff)) #print(adiff) # if dist < g.targetdist or dist < brake_s or missed: if (not g.allangles and abs(adiff) > 90) or dist < g.targetdist: if False: #stop("9") # driving.drive(-1) # continue a little so it control pass the target if it wasn't # there yet time.sleep(0.5) # driving.drive(-1) # time.sleep(0.2) driving.drive(0) #print("adiff %f dist %f" % (adiff, dist)) if dist < g.targetdist: #print("dist < %f" % g.targetdist) pass if abs(adiff) > 90: print("adiff = %f; leaving (%f,%f) behind" % (adiff, x, y)) return 1 return 0 asgn = sign(adiff) aval = abs(adiff) st = g.anglefactor * aval if st > 100: st = 100 st = asgn * g.speedsign * st if False: st_d = st - g.steering if st_d > 10: st = g.steering + 10 elif st_d < -10: st = g.steering - 10 driving.steer(st) tolog("gotoa4 steer %f" % (st)) if not g.simulate: nav_tc.send_to_ground_control( "dpos %f %f %f %f 0 %f" % (g.ppx, g.ppy, g.ang, time.time() - g.t0, g.finspeed)) tt0 = time.time() d = nav_map.roaddist(g.ppx, g.ppy) if d > g.slightlyoffroad: # print("roaddist %f at %f, %f" % (d, g.ppx, g.ppy)) if d > g.maxoffroad: print("roaddist %f at %f, %f" % (d, g.ppx, g.ppy)) return 2 period = 0.1 * g.speedfactor tt1 = time.time() dtt = tt1 - tt0 dtt1 = period - dtt if dtt1 > 0: time.sleep(period)
def goto_1(x, y): g.targetx = x g.targety = y missed = False inc = 0 inc2 = 0 lastdist = None brake_s = 0.0 tolog("goto_1 %f %f" % (x, y)) while True: if g.remote_control: print("remote_control is true") return 2 if not checkpos(): if False: print("checkpos returned False") return 2 if g.poserror: if False: print("positioning system error") tolog("poserr 1") g.limitspeed = 0 time.sleep(10) tolog("poserr 2") g.limitspeed = None print("continuing") g.poserror = False tolog("poserr 3") return 2 if g.obstacle: print("obstacle") return 2 dist = getdist(x, y) if g.inspeed != 0: # Assume we are going in the direction of the target. # At low speeds, braking time is about 1.5 s. brake_s = 1.5 * abs(g.inspeed)/100 # say that braking distance is 1 dm at higher speed, when # braking electrically if g.inspeed > 0: brake_s = 0.4 else: brake_s = 0.6 # we should only use brake_s when we are going to stop brake_s = 0.0 # 'lastdist' is never non-None now, nor 'missed' if lastdist != None: if dist < lastdist - 0.01: inc = -1 lastdist = dist elif dist > lastdist + 0.01: if inc == -1: missed = True tolog("missed target") inc = 1 lastdist = dist tolog("gotoa1 %f %f -> %f %f" % (g.ppx, g.ppy, x, y)) a = atan2(y-g.ppy, x-g.ppx) adeg = 180/pi*a adeg = 90-adeg adiff = g.ang - adeg adiff = adiff%360 tolog("gotoa2 a %f adeg %f adiff %f" % (a, adeg, adiff)) if g.speedsign < 0: adiff += 180 if adiff > 180: adiff -= 360 adiff = -adiff # now, positive means the target is to the right of us tolog("gotoa3 adiff %f" % (adiff)) #print(adiff) # if dist < g.targetdist or dist < brake_s or missed: if (not g.allangles and abs(adiff) > 90) or dist < g.targetdist: if False: #stop("9") # driving.drive(-1) # continue a little so it can pass the target if it wasn't # there yet time.sleep(0.5) # driving.drive(-1) # time.sleep(0.2) driving.drive(0) #print("adiff %f dist %f" % (adiff, dist)) if dist < g.targetdist: #print("dist < %f" % g.targetdist) pass if abs(adiff) > 90: print("adiff = %f; leaving (%f,%f) behind" % (adiff,x,y)) return 1 return 0 asgn = sign(adiff) aval = abs(adiff) st = g.anglefactor*aval if st > 100: st = 100 st = asgn*g.speedsign*st if False: st_d = st - g.steering if st_d > 10: st = g.steering + 10 elif st_d < -10: st = g.steering - 10 driving.steer(st) tolog("gotoa4 steer %f" % (st)) if not g.simulate: nav_tc.send_to_ground_control("dpos %f %f %f %f 0 %f" % ( g.ppx,g.ppy,g.ang,time.time()-g.t0, g.finspeed)) tt0 = time.time() d = nav_map.roaddist(g.ppx, g.ppy) if d > g.slightlyoffroad: # print("roaddist %f at %f, %f" % (d, g.ppx, g.ppy)) if d > g.maxoffroad: print("roaddist %f at %f, %f" % (d, g.ppx, g.ppy)) return 2 period = 0.1*g.speedfactor tt1 = time.time() dtt = tt1-tt0 dtt1 = period - dtt if dtt1 > 0: time.sleep(period)