def drive(sp):
if True:
if sp != 0 and not g.braking:
g.speedsign = sign(sp)
g.outspeedcm = sp*2
#print("outspeedcm = %d" % g.outspeedcm)
else:
# do this in readspeed2 instead
# maybe, but then steer will zero the speed
g.outspeed = sp
if abs(sp) >= 7:
g.speedtime = time.time()
else:
g.speedtime = None
if sp != 0 and not g.braking:
g.speedsign = sign(sp)
if sp < 0:
sp += 256
st = g.steering
if st < 0:
st += 256
tolog("motor %d steer %d" % (sp, g.steering))
cmd = "/home/pi/can-utils/cansend can0 '101#%02x%02x'" % (
sp, st)
#print (sp, g.steering, cmd)
os.system(cmd)
def drive(sp):
if True:
if sp != 0 and not g.braking:
g.speedsign = sign(sp)
g.outspeedcm = sp * 2
#print("outspeedcm = %d" % g.outspeedcm)
else:
# do this in readspeed2 instead
# maybe, but then steer will zero the speed
g.outspeed = sp
if abs(sp) >= 7:
g.speedtime = time.time()
else:
g.speedtime = None
if sp != 0 and not g.braking:
g.speedsign = sign(sp)
if sp < 0:
sp += 256
st = g.steering
if st < 0:
st += 256
tolog("motor %d steer %d" % (sp, g.steering))
cmd = "/home/pi/control-utils/cansend can0 '101#%02x%02x'" % (sp, st)
#print (sp, g.steering, cmd)
os.system(cmd)
def keepspeed():
outspeedi = 0
# 0 to 9
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]
while True:
time.sleep(0.1)
if g.outspeedcm == None:
continue
spi = outspeedi
desiredspeed = g.outspeedcm
if g.limitspeed != None and desiredspeed > g.limitspeed:
desiredspeed = g.limitspeed
if g.user_pause:
desiredspeed = 0
if desiredspeed > g.finspeed:
if spi < len(speeds)-1:
spi += 1
elif desiredspeed < g.finspeed:
if spi > 0:
spi -= 1
desiredspeed_sign = sign(desiredspeed)
desiredspeed_abs = abs(desiredspeed)
if True:
# bypass the control
spi = int(desiredspeed_abs/10)
if spi > len(speeds)-1:
spi = len(speeds)-1
sleeptime = 1
else:
sleeptime = 3
sp = speeds[spi]
outspeedi = spi
# spi/outspeedi don't remember the sign
sp *= desiredspeed_sign
if abs(sp) >= 7:
g.speedtime = time.time()
else:
g.speedtime = None
if g.outspeed == sp and sp != 0:
# pass
continue
if False:
print("outspeedcm %f finspeed %f outspeedi %d spi %d sp %f outspeed %f" % (
g.outspeedcm, g.finspeed, outspeedi, spi, sp, g.outspeed))
g.outspeed = sp
if sp != 0 and not g.braking:
g.speedsign = sign(sp)
if sp != 0:
nav_signal.warningblink(False)
# if sp < 0:
# sp += 256
st = g.steering
# if st < 0:
# st += 256
tolog("motor %d steer %d" % (sp, st))
driving.dodrive(sp, st)
time.sleep(sleeptime)
def keepspeed():
outspeedi = 0
# 0 to 9
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
]
while True:
time.sleep(0.1)
if g.outspeedcm == None:
continue
spi = outspeedi
desiredspeed = g.outspeedcm
if g.limitspeed != None and desiredspeed > g.limitspeed:
desiredspeed = g.limitspeed
if g.user_pause:
desiredspeed = 0
if desiredspeed > g.finspeed:
if spi < len(speeds) - 1:
spi += 1
elif desiredspeed < g.finspeed:
if spi > 0:
spi -= 1
desiredspeed_sign = sign(desiredspeed)
desiredspeed_abs = abs(desiredspeed)
if True:
# bypass the control
spi = int(desiredspeed_abs / 10)
if spi > len(speeds) - 1:
spi = len(speeds) - 1
sleeptime = 1
else:
sleeptime = 3
sp = speeds[spi]
outspeedi = spi
# spi/outspeedi don't remember the sign
sp *= desiredspeed_sign
if False:
print(
"outspeedcm %f finspeed %f outspeedi %d spi %d sp %f outspeed %f"
% (g.outspeedcm, g.finspeed, outspeedi, spi, sp, g.outspeed))
if g.outspeed == sp and sp != 0:
# pass
continue
g.outspeed = sp
if abs(sp) >= 7:
g.speedtime = time.time()
else:
g.speedtime = None
if sp != 0 and not g.braking:
g.speedsign = sign(sp)
if sp != 0:
nav_signal.warningblink(False)
# if sp < 0:
# sp += 256
st = g.steering
# if st < 0:
# st += 256
tolog("motor %d steer %d" % (sp, st))
driving.dodrive(sp, st)
time.sleep(sleeptime)
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)
