def init():
g.VIN = readvin()
print("VIN %s" % g.VIN)
if g.VIN == "car5":
g.mxmin = -99
g.mxmax = -40
g.mymin = 100
g.mymax = 152
if g.VIN == "car3":
g.mxmin = -20
g.mxmax = 39
g.mymin = 37
g.mymax = 85
# fake, just to make them defined
if g.VIN == "car4":
g.mxmin = 0
g.mxmax = 100
g.mymin = 0
g.mymax = 100
canNetwork = "can0"
canFrameID = 1025
g.canSocket = initializeCAN(canNetwork)
g.angleknown = False
eightinit()
setleds(0, 7)
start_new_thread(nav_tc.connect_to_ground_control, ())
g.logf = open("navlog", "w")
g.accf = open("acclog", "w")
#g.accf.write("%f %f %f %f %f %f %f %f %f %f %f %f %f\n" % (
#x, y, g.vx, g.vy, g.px, g.py, x0, y0, vvx, vvy, g.ppx, g.ppy, g.ang))
g.accf.write(
"x y vx vy px py x0 y0 vvx vvy ppx ppy ang angvel steering speed inspeed outspeed odometer z0 r rx ry acc finspeed fodometer t can_ultra\n"
)
g.t0 = time.time()
print("t0 = %f" % g.t0)
tolog("init")
nav_imu.calibrate_imu()
start_new_thread(readmarker, ())
start_new_thread(nav_mqtt.handle_mqtt, ())
start_new_thread(readspeed2, ())
start_new_thread(nav_imu.readgyro, ())
start_new_thread(senddrive, ())
start_new_thread(keepspeed, ())
start_new_thread(heartbeat, ())
start_new_thread(connect_to_ecm, ())
def init():
g.VIN = readvin()
print("VIN %s" % g.VIN)
if g.VIN == "car5":
g.mxmin = -99
g.mxmax = -40
g.mymin = 100
g.mymax = 152
if g.VIN == "car3":
g.mxmin = -20
g.mxmax = 39
g.mymin = 37
g.mymax = 85
# fake, just to make them defined
if g.VIN == "car4":
g.mxmin = 0
g.mxmax = 100
g.mymin = 0
g.mymax = 100
canNetwork = "can0"
canFrameID = 1025
g.canSocket = initializeCAN(canNetwork)
g.angleknown = False
eightinit()
setleds(0, 7)
start_new_thread(nav_tc.connect_to_ground_control, ())
g.logf = open("navlog", "w")
g.accf = open("acclog", "w")
#g.accf.write("%f %f %f %f %f %f %f %f %f %f %f %f %f\n" % (
#x, y, g.vx, g.vy, g.px, g.py, x0, y0, vvx, vvy, g.ppx, g.ppy, g.ang))
g.accf.write("x y vx vy px py x0 y0 vvx vvy ppx ppy ang angvel steering speed inspeed outspeed odometer z0 r rx ry acc finspeed fodometer t can_ultra\n")
g.t0 = time.time()
print("t0 = %f" % g.t0)
tolog("init")
nav_imu.calibrate_imu()
start_new_thread(readmarker, ())
start_new_thread(nav_mqtt.handle_mqtt, ())
start_new_thread(readspeed2, ())
start_new_thread(nav_imu.readgyro, ())
start_new_thread(senddrive, ())
start_new_thread(keepspeed, ())
start_new_thread(heartbeat, ())
start_new_thread(connect_to_ecm, ())
def reset():
g.remote_control = False
g.rc_button = False
g.warningblinking = False
nav_signal.setleds(0,7)
g.markerno = 0
g.markercnt = 0
g.angleknown = False
g.crash = False
g.crashacc = None
def reset():
g.remote_control = False
g.rc_button = False
g.warningblinking = False
nav_signal.setleds(0, 7)
g.markerno = 0
g.markercnt = 0
g.angleknown = False
g.crash = False
g.crashacc = None
def trip(path, first=0):
g.speakcount = 1
setleds(0, 7)
i = 0
while True:
if g.rc_button:
setleds(1, 6)
stop()
break
j = 0
if first > 0:
path1 = path[first:]
else:
path1 = path
for cmd in path1:
if cmd[0] == 'go':
sp = cmd[1]
sp = int(sp * g.parameter / 100.0)
x = cmd[2]
y = cmd[3]
spdiff = sp - g.outspeed
if spdiff > 20:
# this should be done in a separate thread
drive(sp - spdiff / 2)
time.sleep(0.5)
drive(sp)
else:
drive(sp)
goto_1(x, y)
elif cmd[0] == 'stop':
if len(cmd) > 1:
t = float(cmd[1])
else:
t = 3
g.paused = True
stopx(i, t)
send_to_ground_control("stopat %d" % j)
while g.paused:
time.sleep(1)
elif cmd[0] == 'speak':
speak(cmd[1])
else:
print("unknown path command: %s" % cmd)
j += 1
first = 0
g.speakcount += 1
def trip(path, first=0):
g.speakcount = 1
setleds(0, 7)
i = 0
while True:
if g.rc_button:
setleds(1, 6)
stop()
break
j = 0
if first > 0:
path1 = path[first:]
else:
path1 = path
for cmd in path1:
if cmd[0] == 'go':
sp = cmd[1]
sp = int(sp*g.parameter/100.0)
x = cmd[2]
y = cmd[3]
spdiff = sp-g.outspeed
if spdiff > 20:
# this should be done in a separate thread
drive(sp-spdiff/2)
time.sleep(0.5)
drive(sp)
else:
drive(sp)
goto_1(x, y)
elif cmd[0] == 'stop':
if len(cmd) > 1:
t = float(cmd[1])
else:
t = 3
g.paused = True
stopx(i, t)
send_to_ground_control("stopat %d" % j)
while g.paused:
time.sleep(1)
elif cmd[0] == 'speak':
speak(cmd[1])
else:
print("unknown path command: %s" % cmd)
j += 1
first = 0
g.speakcount += 1
def braketest(v0, v1):
steer(-100)
nav_signal.setleds(0,7)
drive(v0/3)
time.sleep(2)
drive(v0*2/3)
time.sleep(2)
drive(v0)
time.sleep(10)
while True:
time.sleep(0.0001)
if g.ang%360 < 5:
nav_signal.setleds(0,0)
o0 = g.odometer
a0 = g.ang
drive(v1)
break
time.sleep(5)
o1 = g.odometer
a1 = g.ang
print((o1-o0, a1-a0))
def braketest(v0, v1):
steer(-100)
nav_signal.setleds(0, 7)
drive(v0 / 3)
time.sleep(2)
drive(v0 * 2 / 3)
time.sleep(2)
drive(v0)
time.sleep(10)
while True:
time.sleep(0.0001)
if g.ang % 360 < 5:
nav_signal.setleds(0, 0)
o0 = g.odometer
a0 = g.ang
drive(v1)
break
time.sleep(5)
o1 = g.odometer
a1 = g.ang
print((o1 - o0, a1 - a0))
def init():
random.seed(g.VIN)
if g.VIN == "car5":
g.mxmin = -99
g.mxmax = -40
g.mymin = 100
g.mymax = 152
if g.VIN == "car3":
g.mxmin = -20
g.mxmax = 39
g.mymin = 37
g.mymax = 85
# fake, just to make them defined
if g.VIN == "car4":
g.mxmin = 0
g.mxmax = 100
g.mymin = 0
g.mymax = 100
canNetwork = "can0"
canFrameID = 1025
if not g.simulate:
g.canSocket = initializeCAN(canNetwork)
g.angleknown = False
eight.eightinit()
g.t0 = time.time()
nav_signal.setleds(0, 7)
suffix = ""
if g.simulate:
suffix = "-" + g.VIN
g.logf = open("navlog" + suffix, "w")
g.accf = open("acclog" + suffix, "w", 1024)
if not g.standalone:
start_new_thread(nav_tc.connect_to_ground_control, ())
#g.accf.write("%f %f %f %f %f %f %f %f %f %f %f %f\n" % (
#x, y, g.vx, g.vy, g.px, g.py, x0, y0, vvx, vvy, g.ppx, g.ppy, g.ang))
g.accf.write("x y vx vy px py x0 y0 vvx vvy ppx ppy ang angvel steering speed inspeed outspeed odometer z0 r rx ry acc finspeed fodometer t pleftspeed leftspeed fleftspeed realspeed can_ultra droppedlog\n")
g.accfqsize = 1000
g.accfq = queue.Queue(g.accfqsize)
start_new_thread(nav_log.logthread, (g.accfq,))
g.qlen = 0
tolog("t0 = %f" % g.t0)
tolog("init")
if not g.simulate:
nav_imu.calibrate_imu()
if not g.simulate:
start_new_thread(wm.readmarker, ())
# if not g.simulate:
start_new_thread(nav_mqtt.handle_mqtt, ())
if not g.simulate:
start_new_thread(wm.readspeed2, ())
if not g.simulate:
start_new_thread_really(nav_imu.readgyro, ())
if not g.simulate:
start_new_thread(driving.senddrive, ())
if not g.simulate:
start_new_thread(keepspeed, ())
if not g.standalone:
start_new_thread(heartbeat, ())
if not g.simulate:
start_new_thread(connect_to_ecm, ())
if not g.simulate:
start_new_thread(pos_thread, ())
if g.simulate:
g.steering = 0
g.finspeed = 0
start_new_thread(wm.simulatecar, ())
def init():
random.seed(g.VIN)
if g.VIN == "car5":
g.mxmin = -99
g.mxmax = -40
g.mymin = 100
g.mymax = 152
if g.VIN == "car3":
g.mxmin = -20
g.mxmax = 39
g.mymin = 37
g.mymax = 85
# fake, just to make them defined
if g.VIN == "car4":
g.mxmin = 0
g.mxmax = 100
g.mymin = 0
g.mymax = 100
canNetwork = "can0"
canFrameID = 1025
if not g.simulate:
g.canSocket = initializeCAN(canNetwork)
g.angleknown = False
eight.eightinit()
g.t0 = time.time()
nav_signal.setleds(0, 7)
start_new_thread(nav_tc.connect_to_ground_control, ())
suffix = ""
if g.simulate:
suffix = "-" + g.VIN
g.logf = open("navlog" + suffix, "w")
g.accf = open("acclog" + suffix, "w", 1024)
#g.accf.write("%f %f %f %f %f %f %f %f %f %f %f %f\n" % (
#x, y, g.vx, g.vy, g.px, g.py, x0, y0, vvx, vvy, g.ppx, g.ppy, g.ang))
g.accf.write(
"x y vx vy px py x0 y0 vvx vvy ppx ppy ang angvel steering speed inspeed outspeed odometer z0 r rx ry acc finspeed fodometer t pleftspeed leftspeed fleftspeed realspeed can_ultra droppedlog\n"
)
g.accfqsize = 1000
g.accfq = queue.Queue(g.accfqsize)
start_new_thread(nav_log.logthread, (g.accfq, ))
g.qlen = 0
tolog("t0 = %f" % g.t0)
tolog("init")
if not g.simulate:
nav_imu.calibrate_imu()
if not g.simulate:
start_new_thread(wm.readmarker, ())
if not g.simulate:
start_new_thread(nav_mqtt.handle_mqtt, ())
if not g.simulate:
start_new_thread(wm.readspeed2, ())
if not g.simulate:
start_new_thread(nav_imu.readgyro, ())
if not g.simulate:
start_new_thread(driving.senddrive, ())
if not g.simulate:
start_new_thread(keepspeed, ())
start_new_thread(heartbeat, ())
if not g.simulate:
start_new_thread(connect_to_ecm, ())
if g.simulate:
g.steering = 0
g.finspeed = 0
start_new_thread(wm.simulatecar, ())