def init_obd_can(args):
"""
If an OBD CAN channel is specified, initialize a bus object.
"""
return (CanBus(vehicle=args['--vehicle'],
bustype=args['--bustype'],
channel=args['--carcan']) if args['--carcan'] else None)
def main(args):
if args['--version']:
print('oscc-check 0.0.2')
return
check_vehicle_arg(args['--vehicle'])
bus = CanBus(
vehicle=args['--vehicle'],
bustype=args['--bustype'],
channel=args['--channel'])
brakes = OsccModule(base_arbitration_id=0x70, module_name='brake')
steering = OsccModule(base_arbitration_id=0x80, module_name='steering')
throttle = OsccModule(base_arbitration_id=0x90, module_name='throttle')
modules = DebugModules(bus, brakes, steering, throttle)
# Initialize module for printing colored text to stdout
colorama.init()
if args['--disable']:
modules.disable()
return
elif args['--enable']:
modules.enable()
return
# Each section or step of the following loop is distinguished from the next by this separator.
# The output begins with this separator for visually consistent output.
print("|Enable Modules -----------------------------------------------------------------|")
# This `while` loop repeats the same basic steps on each iteration, they are as follows:
# 1. Enable each OSCC module.
# 2. Send commands to increase and decrease brake pressure.
# 3. Verify that brake pressure reported by vehicle increased or decreased accordingly.
# 4. Send commands to apply positive or negative torque to steering wheel.
# 5. Verify that the steering wheel angle increased or decreased accordingly.
# 6. Disable each OSCC module.
while True:
modules.enable()
# Visually distinguish enable steps from the following brake validation
print("|Brake Test --------------------------------------------------------------------|")
pressure_cmd = 0.0
modules.command_brake_module(pressure_cmd, expect=None)
pressure_cmd = 0.5
modules.command_brake_module(pressure_cmd, expect='increase')
pressure_cmd = 0.0
modules.command_brake_module(pressure_cmd, expect='decrease')
pressure_cmd = 0.3
modules.command_brake_module(pressure_cmd, expect='increase')
pressure_cmd = 0.0
modules.command_brake_module(pressure_cmd, expect='decrease')
# Visually distinguish brake validation from the following steering wheel validation
print("|Steering Test ------------------------------------------------------------------|")
torque_cmd = -0.1
modules.command_steering_module(torque_cmd, expect=None)
torque_cmd = 0.1
modules.command_steering_module(torque_cmd, expect=None)
torque_cmd = 0.15
modules.command_steering_module(torque_cmd, expect='increase')
torque_cmd = -0.15
modules.command_steering_module(torque_cmd, expect='decrease')
# Visually distinguish throttle validation from the following disable steps
print("|Disable Modules ----------------------------------------------------------------|")
modules.disable()
if not args['--loop']:
break
# Visually distinguish disable steps from the following enable steps
print("|Enable Modules -----------------------------------------------------------------|")
def main(args):
if args['--version']:
print('oscc-check 0.0.2')
return
check_vehicle_arg(args['--vehicle'])
bus = CanBus(vehicle=args['--vehicle'],
bustype=args['--bustype'],
channel=args['--channel'])
brakes = OsccModule(base_arbitration_id=0x70, module_name='brake')
steering = OsccModule(base_arbitration_id=0x80, module_name='steering')
throttle = OsccModule(base_arbitration_id=0x90, module_name='throttle')
#steering_angle = OsccModule(base_arbitration_id=0x2B0, module_name="steering_angle")
modules = DebugModules(bus, brakes, steering, throttle)
# Initialize module for printing colored text to stdout
colorama.init()
if args['--disable']:
modules.disable()
return
elif args['--enable']:
modules.enable()
return
# Each section or step of the following loop is distinguished from the next by this separator.
# The output begins with this separator for visually consistent output.
print(
"|Enable Modules -----------------------------------------------------------------|"
)
# This `while` loop repeats the same basic steps on each iteration, they are as follows:
# 1. Enable each OSCC module.
# 2. Send commands to increase and decrease brake pressure.
# 3. Verify that brake pressure reported by vehicle increased or decreased accordingly.
# 4. Send commands to apply positive or negative torque to steering wheel.
# 5. Verify that the steering wheel angle increased or decreased accordingly.
# 6. Disable each OSCC module.
while True:
modules.enable()
# Visually distinguish brake validation from the following steering wheel validation
print(
"|Steering Test ------------------------------------------------------------------|"
)
STEERING_RATIO = 1 / 15.7
file_num = len(os.listdir("tests")) + 7
fieldnames = ["Torque", "New Angle", "Change in Angle", "Goal Angle"]
with open("torque_test_space2{}.csv".format(file_num), "w") as csvfile:
writer = csv.DictWriter(csvfile, fieldnames=fieldnames)
writer.writeheader()
angles = [modules.bus.check_steering_wheel_angle().value]
current = 0
max_torque = 0.25
step = 0.05
previous = 0
num_steps = max_torque / step + 1
i = -1
for n in range(int(num_steps)):
for j in range(3):
if j == 0:
print("j = 0")
elif j == 1:
print("j = 1")
elif j == 2:
print("j = 2")
if j == 0:
for k in range(3):
torque_cmd = current
print("torque = " + str(torque_cmd))
try:
angles.append(
modules.command_steering_module(
torque_cmd, expect=None))
except:
raise Exception(
"Steering angle function error")
writer.writerow({
"Torque":
torque_cmd,
"New Angle":
angles[i],
"Change in Angle":
angles[i] - angles[i - 1],
"Goal Angle":
"n/a"
})
elif j == 1:
if current != 0:
for k in range(3):
torque_cmd = 0
print("torque = " + str(torque_cmd))
try:
angles.append(
modules.command_steering_module(
torque_cmd, expect=None))
except:
raise Exception(
"Steering angle function error")
writer.writerow({
"Torque":
torque_cmd,
"New Angle":
angles[i],
"Change in Angle":
angles[i] - angles[i - 1],
"Goal Angle":
"n/a"
})
else:
print("already tested 0")
elif j == 2:
if current != 0:
for k in range(3):
torque_cmd = -current
print("torque = " + str(torque_cmd))
try:
angles.append(
modules.command_steering_module(
torque_cmd, expect=None))
except:
raise Exception(
"Steering angle function error")
writer.writerow({
"Torque":
torque_cmd,
"New Angle":
angles[i],
"Change in Angle":
angles[i] - angles[i - 1],
"Goal Angle":
"n/a"
})
else:
print("already tested 0")
if -max_torque < current < max_torque:
current += step
'''
PolySync's Original Test Code
torque_cmd = -0.1
modules.command_steering_module(torque_cmd, expect=None)
torque_cmd = 0.1
modules.command_steering_module(torque_cmd, expect=None)
torque_cmd = 0.15
modules.command_steering_module(torque_cmd, expect='increase')
torque_cmd = -0.15
modules.command_steering_module(torque_cmd, expect='decrease')
'''
# Visually distinguish enable steps from the following brake validation
print(
"|Brake Test --------------------------------------------------------------------|"
)
pressure_cmd = 0.0
modules.command_brake_module(pressure_cmd, expect=None)
pressure_cmd = 0.5
modules.command_brake_module(pressure_cmd, expect='increase')
pressure_cmd = 0.0
modules.command_brake_module(pressure_cmd, expect='decrease')
pressure_cmd = 0.3
modules.command_brake_module(pressure_cmd, expect='increase')
pressure_cmd = 0.0
modules.command_brake_module(pressure_cmd, expect='decrease')
# Visually distinguish throttle validation from the following disable steps
print(
"|Disable Modules ----------------------------------------------------------------|"
)
modules.disable()
if not args['--loop']:
break
# Visually distinguish disable steps from the following enable steps
print(
"|Enable Modules -----------------------------------------------------------------|"
)
def main(args):
# if args['--version']:
# print('oscc-check 0.0.2')
# return
# check_vehicle_arg(args['--vehicle'])
bus = CanBus(vehicle=args['--vehicle'],
bustype=args['--bustype'],
channel=args['--channel'])
brakes = OsccModule(base_arbitration_id=0x70, module_name='brake')
steering = OsccModule(base_arbitration_id=0x80, module_name='steering')
throttle = OsccModule(base_arbitration_id=0x90, module_name='throttle')
modules = DebugModules(bus, brakes, steering, throttle)
# Initialize module for printing colored text to stdout
colorama.init()
if args['--disable']:
modules.disable()
return
elif args['--enable']:
modules.enable()
return
# Each section or step of the following loop is distinguished from the next by this separator.
# The output begins with this separator for visually consistent output.
print(
"|Enable Modules -----------------------------------------------------------------|"
)
modules.enable()
screen = pygame.display.set_mode([width, height])
pygame.display.set_caption("Autonomous agents")
clock = pygame.time.Clock()
steer = Steer(screen=screen)
kp = 0.5e-2 #0.375e-2
ki = 2e-3 #1.5e-3
kd = 0e-3
pid = PID(P=kp, I=ki, D=kd, output_min=-0.33, output_max=0.33)
pid.setWindup(100)
t_start = time.time()
current_angle_deque = collections.deque(maxlen=2)
t_last = t_start
target_angle = modules.read_steering_module()
throttle_command = 0
brake_command = 0
while True:
t_now = time.time()
print(" frequency: ", 1 / (t_now - t_last))
t_last = t_now
mainScreenPressed = pygame.key.get_pressed()
if mainScreenPressed[K_a] and target_angle > -360:
target_angle -= 3
if mainScreenPressed[K_d] and target_angle < 360:
target_angle += 3
current_angle = modules.read_steering_module()
current_angle_deque.append(current_angle)
# clock.tick(30)
screen.fill(WHITE)
steer.show(angle=current_angle)
pygame.display.flip()
for event in pygame.event.get():
if event.type == pygame.QUIT:
done = True
if mainScreenPressed[K_q]:
exit()
pid.update(target_angle, np.mean(current_angle_deque))
control = pid.output
modules.command_steering_module(control)
if mainScreenPressed[K_w]:
throttle_command = min(throttle_command + 8e-3, 0.2) #0.2
else:
throttle_command = 0
if mainScreenPressed[K_s]:
brake_command = min(brake_command + 16e-3, 0.3) #0.3
else:
brake_command = 0
modules.command_throttle_module(throttle_command)
modules.command_brake_module(brake_command)
print(
"|Disable Modules ----------------------------------------------------------------|"
)
modules.disable()