mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-10-01 03:43:46 +08:00
@@ -1,7 +1,6 @@
|
||||
#!/usr/bin/env python
|
||||
import os
|
||||
import sys
|
||||
import struct
|
||||
from collections import defaultdict
|
||||
from common.realtime import sec_since_boot
|
||||
import zmq
|
||||
|
||||
@@ -19,7 +19,8 @@ msgs = {}
|
||||
while True:
|
||||
lc = messaging.recv_sock(logcan, True)
|
||||
for c in lc.can:
|
||||
if c.src == 0:
|
||||
# read also msgs sent by EON on CAN bus 0x80
|
||||
if c.src%0x80 == 0:
|
||||
msgs[c.address] = len(c.dat)
|
||||
|
||||
fingerprint = ', '.join("%d: %d" % v for v in sorted(msgs.items()))
|
||||
|
||||
@@ -1,9 +1,7 @@
|
||||
#!/usr/bin/env python
|
||||
import os
|
||||
import time
|
||||
import subprocess
|
||||
from cffi import FFI
|
||||
import ctypes
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
@@ -1,92 +0,0 @@
|
||||
#!/usr/bin/env python
|
||||
import time
|
||||
import numpy as np
|
||||
import zmq
|
||||
from evdev import InputDevice
|
||||
from select import select
|
||||
|
||||
from cereal import car
|
||||
from common.realtime import Ratekeeper
|
||||
|
||||
import selfdrive.messaging as messaging
|
||||
from selfdrive.services import service_list
|
||||
from selfdrive.car import get_car
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
# ***** connect to joystick *****
|
||||
# we use a Mad Catz V.1
|
||||
dev = InputDevice("/dev/input/event8")
|
||||
print dev
|
||||
|
||||
button_values = [0]*7
|
||||
axis_values = [0.0, 0.0, 0.0]
|
||||
|
||||
# ***** connect to car *****
|
||||
context = zmq.Context()
|
||||
logcan = messaging.sub_sock(context, service_list['can'].port)
|
||||
sendcan = messaging.pub_sock(context, service_list['sendcan'].port)
|
||||
|
||||
CI, CP = get_car(logcan, sendcan)
|
||||
CC = car.CarControl.new_message()
|
||||
|
||||
rk = Ratekeeper(100)
|
||||
|
||||
while 1:
|
||||
# **** handle joystick ****
|
||||
r, w, x = select([dev], [], [], 0.0)
|
||||
if dev in r:
|
||||
for event in dev.read():
|
||||
# button event
|
||||
if event.type == 1:
|
||||
btn = event.code - 288
|
||||
if btn >= 0 and btn < 7:
|
||||
button_values[btn] = int(event.value)
|
||||
|
||||
# axis move event
|
||||
if event.type == 3:
|
||||
if event.code < 3:
|
||||
if event.code == 2:
|
||||
axis_values[event.code] = np.clip((255-int(event.value))/250.0, 0.0, 1.0)
|
||||
else:
|
||||
DEADZONE = 5
|
||||
if event.value-DEADZONE < 128 and event.value+DEADZONE > 128:
|
||||
event.value = 128
|
||||
axis_values[event.code] = np.clip((int(event.value)-128)/120.0, -1.0, 1.0)
|
||||
|
||||
print axis_values, button_values
|
||||
# **** handle car ****
|
||||
|
||||
CS = CI.update(CC)
|
||||
#print CS
|
||||
CC = car.CarControl.new_message()
|
||||
|
||||
|
||||
CC.enabled = True
|
||||
|
||||
CC.gas = float(np.clip(-axis_values[1], 0, 1.0))
|
||||
CC.brake = float(np.clip(axis_values[1], 0, 1.0))
|
||||
CC.steeringTorque = float(-axis_values[0])
|
||||
|
||||
CC.hudControl.speedVisible = bool(button_values[1])
|
||||
CC.hudControl.lanesVisible = bool(button_values[2])
|
||||
CC.hudControl.leadVisible = bool(button_values[3])
|
||||
|
||||
CC.cruiseControl.override = bool(button_values[0])
|
||||
CC.cruiseControl.cancel = bool(button_values[-1])
|
||||
|
||||
CC.hudControl.setSpeed = float(axis_values[2] * 100.0)
|
||||
|
||||
# TODO: test alerts
|
||||
CC.hudControl.visualAlert = "none"
|
||||
CC.hudControl.audibleAlert = "none"
|
||||
|
||||
#print CC
|
||||
|
||||
if not CI.apply(CC):
|
||||
print "CONTROLS FAILED"
|
||||
|
||||
rk.keep_time()
|
||||
|
||||
|
||||
|
||||
@@ -1,52 +0,0 @@
|
||||
#!/usr/bin/env python
|
||||
import os
|
||||
import zmq
|
||||
from cereal import car
|
||||
|
||||
import selfdrive.messaging as messaging
|
||||
from selfdrive.services import service_list
|
||||
|
||||
from selfdrive.car import get_car
|
||||
|
||||
def bpressed(CS, btype):
|
||||
for b in CS.buttonEvents:
|
||||
if b.type == btype:
|
||||
return True
|
||||
return False
|
||||
|
||||
def test_loop():
|
||||
context = zmq.Context()
|
||||
logcan = messaging.sub_sock(context, service_list['can'].port)
|
||||
|
||||
CI, CP = get_car(logcan)
|
||||
|
||||
state = 0
|
||||
|
||||
states = [
|
||||
"'seatbeltNotLatched' in CS.errors",
|
||||
"CS.gasPressed",
|
||||
"CS.brakePressed",
|
||||
"CS.steeringPressed",
|
||||
"bpressed(CS, 'leftBlinker')",
|
||||
"bpressed(CS, 'rightBlinker')",
|
||||
"bpressed(CS, 'cancel')",
|
||||
"bpressed(CS, 'accelCruise')",
|
||||
"bpressed(CS, 'decelCruise')",
|
||||
"bpressed(CS, 'altButton1')",
|
||||
"'doorOpen' in CS.errors",
|
||||
"False"]
|
||||
|
||||
while 1:
|
||||
CC = car.CarControl.new_message()
|
||||
# read CAN
|
||||
CS = CI.update(CC)
|
||||
|
||||
while eval(states[state]) == True:
|
||||
state += 1
|
||||
|
||||
print "IN STATE %d: waiting for %s" % (state, states[state])
|
||||
#print CS
|
||||
|
||||
if __name__ == "__main__":
|
||||
test_loop()
|
||||
|
||||
Reference in New Issue
Block a user