Skip to content

Commit

Permalink
Add primitive steer tuning script and json params (commaai#130)
Browse files Browse the repository at this point in the history
* Pull Gernby tuning params into kegman.json & add cell phone Python tuner (commaai#126)

* Add Gernby params to /data/kegman.json config file

* Add Gernby params to JSON part 2

* Add Gernby "live" tuning thru /data/kegman.json

* Fix missing "

* ==

* float()

* Add Gernby tuning Params into /data/kegman.json

* Add Gernby live json tuning params to interface.py

* Add tune.py and tune.sh primitive tuning script

* Update planner.py (commaai#124)

Improve Acceleration

* Turn on tuneGernby to "1" when running tune.py

* Fix kegman.conf for tuneGernby assignment

* Write tuneGernby to JSON

* Remove tuning code - cannot enable live from here

* Extract live tuning params kegman_conf in latcontrol.py

* Put live tune params into Latcontrol

* self.live_tune call

* remove e2front for now

* Fix PL

* don't pull from kegman.json every frame

* mod 300

* mpc_frame init

* Add KpV and KiV to live_tune

* Update latcontrol.py

* change tune vals to -1 to self-populate in latcontrol.py

* add KpV and KiV

* Update kegman_conf.py

* Update kegman_conf.py

* fix steerKf

* write defaults to JSON

* Update latcontrol.py

* Update latcontrol.py

* Update latcontrol.py

* Change to Ki and Kp to avoid var conflict

* Update kegman_conf.py

* Fix retrieval of kegman json object in __init__ latcontrol.py

* Fix quotes around "-1" values in JSON

* Ki and Kp in tune.py instead of KiV and KpV

* Comment out __init__ kegman_conf write troubleshoot

* Update latcontrol.py

* convert kP and kI into arrays?

* Update latcontrol.py

* convert array back into string before writing kP and kI

* Round kI and kP to 2 decimal places

* Press space for next, M for previous element, add 1 and 0

* Remap keys

* Fix indent

* Update tune.py

* Update tune.py

* d g j instead of e t u
  • Loading branch information
kegman authored Mar 10, 2019
1 parent fe0388f commit b1f39cb
Show file tree
Hide file tree
Showing 6 changed files with 182 additions and 7 deletions.
2 changes: 1 addition & 1 deletion selfdrive/car/honda/interface.py
Original file line number Diff line number Diff line change
Expand Up @@ -347,7 +347,7 @@ def get_params(candidate, fingerprint):
ret.longitudinalKiV = [0.18, 0.12]

else:
raise ValueError("unsupported car %s" % candidate)
raise ValueError("unsupported car %s" % candidate)

ret.steerControlType = car.CarParams.SteerControlType.torque

Expand Down
54 changes: 53 additions & 1 deletion selfdrive/controls/lib/latcontrol.py
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@
from cereal import car
import math
import numpy as np
from selfdrive.kegman_conf import kegman_conf

_DT = 0.01 # 100Hz
_DT_MPC = 0.05 # 20Hz
Expand All @@ -25,7 +26,30 @@ def apply_deadzone(angle, deadzone):

class LatControl(object):
def __init__(self, CP):


kegman = kegman_conf()
self.write_conf = False

if kegman.conf['react'] == "-1":
kegman.conf['react'] = str(CP.steerReactance)
self.write_conf = True
if kegman.conf['damp'] == "-1":
kegman.conf['damp'] = str(CP.steerInductance)
self.write_conf = True
if kegman.conf['resist'] == "-1":
kegman.conf['resist'] = str(CP.steerResistance)
self.write_conf = True
if kegman.conf['Kp'] == "-1":
kegman.conf['Kp'] = str(round(CP.steerKpV[0],2))
self.write_conf = True
if kegman.conf['Ki'] == "-1":
kegman.conf['Ki'] = str(round(CP.steerKiV[0],2))
self.write_conf = True

if self.write_conf:
kegman.write_config(kegman.conf)

self.mpc_frame = 0
if CP.steerResistance > 0 and CP.steerReactance >= 0 and CP.steerInductance > 0:
self.smooth_factor = CP.steerInductance * 2.0 * CP.steerActuatorDelay / _DT # Multiplier for inductive component (feed forward)
self.projection_factor = CP.steerReactance * CP.steerActuatorDelay / 2.0 # Mutiplier for reactive component (PI)
Expand Down Expand Up @@ -62,8 +86,36 @@ def __init__(self, CP):
def reset(self):
self.pid.reset()


def live_tune(self, CP):
self.mpc_frame += 1
if self.mpc_frame % 300 == 0:
# live tuning through /data/openpilot/tune.py overrides interface.py settings
kegman = kegman_conf()
if kegman.conf['tuneGernby'] == "1":
self.reactance = float(kegman.conf['react'])
self.inductance = float(kegman.conf['damp'])
self.resistance = float(kegman.conf['resist'])
self.steerKpV = np.array([float(kegman.conf['Kp'])])
self.steerKiV = np.array([float(kegman.conf['Ki'])])

self.accel_limit = 2.0 / self.resistance
self.projection_factor = self.reactance * CP.steerActuatorDelay / 2.0
self.smooth_factor = self.inductance * 2.0 * CP.steerActuatorDelay / _DT

# Eliminate break-points, since they aren't needed (and would cause problems for resonance)
#KpV = [np.interp(25.0, CP.steerKpBP, self.steerKpV)]
KpV = [np.interp(25.0, CP.steerKpBP, self.steerKpV)]
#KiV = [np.interp(25.0, CP.steerKiBP, self.steerKiV)]
KiV = [np.interp(25.0, CP.steerKiBP, self.steerKiV)]
self.pid = PIController(([0.], KpV),
([0.], KiV),
k_f=CP.steerKf, pos_limit=1.0)


def update(self, active, v_ego, angle_steers, angle_rate, angle_offset, steer_override, CP, VM, path_plan):

self.live_tune(CP)
if angle_rate == 0.0 and self.calculate_rate:
if angle_steers != self.prev_angle_steers:
self.steer_counter_prev = self.steer_counter
Expand Down
19 changes: 14 additions & 5 deletions selfdrive/kegman_conf.py
Original file line number Diff line number Diff line change
Expand Up @@ -11,15 +11,21 @@ def read_config(self):
if os.path.isfile('/data/kegman.json'):
with open('/data/kegman.json', 'r') as f:
self.config = json.load(f)

if "battPercOff" not in self.config:
self.config.update({"battPercOff":"25"})
self.element_updated = True
if "carVoltageMinEonShutdown" not in self.config:
self.config.update({"carVoltageMinEonShutdown":"11800"})
self.element_updated = True
if "brakeStoppingTarget" not in self.config:
self.config.update({"brakeStoppingTarget":"0.25"})
self.element_updated = True

if "tuneGernby" not in self.config:
self.config.update({"tuneGernby":"0"})
self.config.update({"react":"-1"})
self.config.update({"damp":"-1"})
self.config.update({"resist":"-1"})
self.config.update({"Kp":"-1"})
self.config.update({"Ki":"-1"})
self.element_updated = True

# Force update battery charge limits to higher values for Big Model
#if self.config['battChargeMin'] != "75":
Expand All @@ -31,7 +37,10 @@ def read_config(self):
self.write_config(self.config)

else:
self.config = {"cameraOffset":"0.06", "lastTrMode":"1", "battChargeMin":"85", "battChargeMax":"90", "wheelTouchSeconds":"180", "battPercOff":"25", "carVoltageMinEonShutdown":"11800", "brakeStoppingTarget":"0.25" }
self.config = {"cameraOffset":"0.06", "lastTrMode":"1", "battChargeMin":"60", "battChargeMax":"70", \
"wheelTouchSeconds":"180", "battPercOff":"25", "carVoltageMinEonShutdown":"11800", \
"brakeStoppingTarget":"0.25", "tuneGernby":"0", "react":"-1", "damp":"-1", \
"resist":"-1", "Kp":"-1", "Ki":"-1"}
self.write_config(self.config)
return self.config

Expand Down
Binary file modified selfdrive/visiond/visiond
Binary file not shown.
113 changes: 113 additions & 0 deletions tune.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,113 @@
from selfdrive.kegman_conf import kegman_conf

letters = { "a":[ "###", "# #", "###", "# #", "# #"], "b":[ "###", "# #", "###", "# #", "###"], "c":[ "###", "#", "#", "#", "###"], "d":[ "##", "# #", "# #", "# #", "##"], "e":[ "###", "#", "###", "#", "###"], "f":[ "###", "#", "###", "#", "#"], "g":[ "###", "# #", "###", " #", "###"], "h":[ "# #", "# #", "###", "# #", "# #"], "i":[ "###", " #", " #", " #", "###"], "j":[ "###", " #", " #", " #", "##"], "k":[ "# #", "##", "#", "##", "# #"], "l":[ "#", "#", "#", "#", "###"], "m":[ "# #", "###", "###", "# #", "# #"], "n":[ "###", "# #", "# #", "# #", "# #"], "o":[ "###", "# #", "# #", "# #", "###"], "p":[ "###", "# #", "###", "#", "#"], "q":[ "###", "# #", "###", " #", " #"], "r":[ "###", "# #", "##", "# #", "# #"], "s":[ "###", "#", "###", " #", "###"], "t":[ "###", " #", " #", " #", " #"], "u":[ "# #", "# #", "# #", "# #", "###"], "v":[ "# #", "# #", "# #", "# #", " #"], "w":[ "# #", "# #", "# #", "###", "###"], "x":[ "# #", " #", " #", " #", "# #"], "y":[ "# #", "# #", "###", " #", "###"], "z":[ "###", " #", " #", "#", "###"], " ":[ " "], "1":[ " #", "##", " #", " #", "###"], "2":[ "###", " #", "###", "#", "###"], "3":[ "###", " #", "###", " #", "###"], "4":[ "#", "#", "# #", "###", " #"], "5":[ "###", "#", "###", " #", "###"], "6":[ "###", "#", "###", "# #", "###"], "7":[ "###", " # ", " #", " #", "#"], "8":[ "###", "# #", "###", "# #", "###"], "9":[ "###", "# #", "###", " #", "###"], "0":[ "###", "# #", "# #", "# #", "###"], "!":[ " # ", " # ", " # ", " ", " # "], "?":[ "###", " #", " ##", " ", " # "], ".":[ " ", " ", " ", " ", " # "], "]":[ " ", " ", " ", " #", " # "], "/":[ " #", " #", " # ", "# ", "# "], ":":[ " ", " # ", " ", " # ", " "], "@":[ "###", "# #", "## ", "# ", "###"], "'":[ " # ", " # ", " ", " ", " "], "#":[ " # ", "###", " # ", "###", " # "], "-":[ " ", " ","###"," "," "] }
# letters stolen from here: http://www.stuffaboutcode.com/2013/08/raspberry-pi-minecraft-twitter.html

def print_letters(text):
bigletters = []
for i in text:
bigletters.append(letters.get(i.lower(),letters[' ']))
output = ['']*5
for i in range(5):
for j in bigletters:
temp = ' '
try:
temp = j[i]
except:
pass
temp += ' '*(5-len(temp))
temp = temp.replace(' ',' ')
temp = temp.replace('#','@')
output[i] += temp
return '\n'.join(output)
import sys, termios, tty, os, time

def getch():
fd = sys.stdin.fileno()
old_settings = termios.tcgetattr(fd)
try:
tty.setraw(sys.stdin.fileno())
ch = sys.stdin.read(1)

finally:
termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
return ch

button_delay = 0.2

kegman = kegman_conf()
#kegman.conf['tuneGernby'] = "1"
#kegman.write_config(kegman.conf)
param = ["tuneGernby", "react", "damp", "resist", "Kp", "Ki"]

j = 0
while True:
print ""
print ""
print print_letters(param[j])
print ""
print print_letters(kegman.conf[param[j]])
print ""
print ("Press 7, 5, 3 to increase by 0.1, 0.05, 0.01")
print ("press d, g, j to decrease by 0.1, 0.05, 0.01")
print ("press 0 to make the value 0")
print ("press 1 to make the value 1")
print ("press SPACE for next parameter")
print ("press m for previous parameter")
print ("press q to quit")

char = getch()
if (char == "7"):
kegman.conf[param[j]] = str(float(kegman.conf[param[j]]) + 0.01)
kegman.write_config(kegman.conf)
time.sleep(button_delay)

elif (char == "5"):
kegman.conf[param[j]] = str(float(kegman.conf[param[j]]) + 0.05)
kegman.write_config(kegman.conf)
time.sleep(button_delay)

elif (char == "3"):
kegman.conf[param[j]] = str(float(kegman.conf[param[j]]) + 0.1)
kegman.write_config(kegman.conf)
time.sleep(button_delay)

elif (char == "j"):
kegman.conf[param[j]] = str(float(kegman.conf[param[j]]) - 0.01)
kegman.write_config(kegman.conf)
time.sleep(button_delay)

elif (char == "g"):
kegman.conf[param[j]] = str(float(kegman.conf[param[j]]) - 0.05)
kegman.write_config(kegman.conf)
time.sleep(button_delay)

elif (char == "d"):
kegman.conf[param[j]] = str(float(kegman.conf[param[j]]) - 0.1)
kegman.write_config(kegman.conf)
time.sleep(button_delay)

elif (char == "0"):
kegman.conf[param[j]] = "0"
kegman.write_config(kegman.conf)
time.sleep(button_delay)

elif (char == "1"):
kegman.conf[param[j]] = "1"
kegman.write_config(kegman.conf)
time.sleep(button_delay)

elif (char == " "):
if j < len(param) - 1:
j = j + 1
else:
j = 0

elif (char == "m"):
if j > 0:
j = j - 1
else:
j = len(param) - 1

elif (char == "q"):
break
1 change: 1 addition & 0 deletions tune.sh
Original file line number Diff line number Diff line change
@@ -0,0 +1 @@
python2.7 tune.py

0 comments on commit b1f39cb

Please sign in to comment.