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