def adapt_steering(position):
"""
Calculates a steer-signal, sets steerdirection
and prints the calculated value
"""
center = RESOLUTIONX / 2
offset = center - position
steerfactor = offset / 10
CRL_CAR.steer(steerfactor)
logging.info(position)
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 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 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 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)
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)
