From a58ec73e334b0fabcb221f2c90a43ff108436161 Mon Sep 17 00:00:00 2001 From: kegman <8837066+kegman@users.noreply.github.com> Date: Wed, 1 May 2019 11:59:12 -0400 Subject: [PATCH] Live PID tuning for standard branch (#160) * Live tuning for Kp and Ki for standard branch * import numpy * Params Kp and Ki only and fix kegman import * initialize self.mpc_frame * Remove Dashboard * Update latcontrol.py * Update latcontrol.py * Update latcontrol.py * Update latcontrol.py * Update latcontrol.py * Update latcontrol.py * Update latcontrol.py --- selfdrive/controls/lib/latcontrol.py | 24 ++++ selfdrive/kegman_conf.py | 70 +++++++++-- selfdrive/tune.py | 157 +++++++++++++++++++++++++ tune.py | 167 +++++++++++++++++++++++++++ tune.sh | 1 + 5 files changed, 410 insertions(+), 9 deletions(-) create mode 100644 selfdrive/tune.py create mode 100644 tune.py create mode 100755 tune.sh diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index 834967584c71de..9a213897ad2c0c 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -1,6 +1,7 @@ from selfdrive.controls.lib.pid import PIController from common.numpy_fast import interp from cereal import car +from selfdrive.kegman_conf import kegman_conf _DT = 0.01 # 100Hz _DT_MPC = 0.05 # 20Hz @@ -17,11 +18,34 @@ def __init__(self, CP): k_f=CP.steerKf, pos_limit=1.0) self.last_cloudlog_t = 0.0 self.angle_steers_des = 0. + self.mpc_frame = 0 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.steerKpV = [float(kegman.conf['Kp'])] + self.steerKiV = [float(kegman.conf['Ki'])] + #self.steerKpV = float(kegman.conf['Kp']) + #self.steerKiV = float(kegman.conf['Ki']) + #self.steerKpV = [0.5] + #self.steerKiV = [0.22] + print "self.steerKpV = ", self.steerKpV + print "self.steerKiV = ", self.steerKiV + self.pid = PIController((CP.steerKpBP, self.steerKpV), + (CP.steerKiBP, self.steerKiV), + k_f=CP.steerKf, pos_limit=1.0) + self.mpc_frame = 0 + def update(self, active, v_ego, angle_steers, steer_override, CP, VM, path_plan): + + self.live_tune(CP) + if v_ego < 0.3 or not active: output_steer = 0.0 self.pid.reset() diff --git a/selfdrive/kegman_conf.py b/selfdrive/kegman_conf.py index 1b636864173b6c..f97cd1caf07ad7 100644 --- a/selfdrive/kegman_conf.py +++ b/selfdrive/kegman_conf.py @@ -2,8 +2,36 @@ import os class kegman_conf(): - def __init__(self): + def __init__(self, CP=None): self.conf = self.read_config() + if CP is not None: + self.init_config(CP) + + def init_config(self, CP): + write_conf = False + if self.conf['tuneGernby'] != "1": + self.conf['tuneGernby'] = str(1) + write_conf = True + if self.conf['reactMPC'] == "-1" or self.conf['dampMPC'] == "-1": + self.conf['reactMPC'] = str(round(CP.steerMPCReactTime,3)) + self.conf['dampMPC'] = str(round(CP.steerMPCDampTime,3)) + write_conf = True + if self.conf['reactSteer'] == "-1" or self.conf['dampSteer'] == "-1": + self.conf['reactSteer'] = str(round(CP.steerReactTime,3)) + self.conf['dampSteer'] = str(round(CP.steerDampTime,3)) + write_conf = True + if self.conf['Kp'] == "-1": + self.conf['Kp'] = str(round(CP.steerKpV[0],3)) + write_conf = True + if self.conf['Ki'] == "-1": + self.conf['Ki'] = str(round(CP.steerKiV[0],3)) + write_conf = True + if self.conf['rateFF'] == "-1": + self.conf['rateFF'] = str(round(CP.rateFFGain,3)) + write_conf = True + + if write_conf: + self.write_config(self.config) def read_config(self): self.element_updated = False @@ -11,30 +39,54 @@ 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 "grafanaUser" not in self.config: + self.config.update({"grafanaUser":"noUser"}) + self.element_updated = True + 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":"1"}) + self.config.update({"Kp":"-1"}) + self.config.update({"Ki":"-1"}) + self.config.update({"dampMPC":"-1"}) + self.config.update({"reactMPC":"-1"}) + if "leadDistance" not in self.config: self.config.update({"leadDistance":"5.0"}) self.element_updated = True - + + if "rateFF" not in self.config: + self.config.update({"rateFF":"-1"}) + self.element_updated = True + + if "dampSteer" not in self.config: + self.config.update({"dampSteer":"-1"}) + self.config.update({"reactSteer":"-1"}) + self.element_updated = True + + # Force update battery charge limits to higher values for Big Model #if self.config['battChargeMin'] != "75": # self.config.update({"battChargeMin":"75"}) # self.config.update({"battChargeMax":"80"}) # self.element_updated = True - - if self.element_updated: + + if self.element_updated: + print("updated") 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", "leadDistance":"5.0" } + self.config = {"cameraOffset":"0.06", "lastTrMode":"1", "battChargeMin":"60", "battChargeMax":"70", \ + "wheelTouchSeconds":"180", "battPercOff":"25", "carVoltageMinEonShutdown":"11800", \ + "brakeStoppingTarget":"0.25", "tuneGernby":"1", "reactMPC":"-1", "reactSteer":"-1", \ + "dampMPC":"-1", "dampSteer":"-1", "Kp":"-1", "Ki":"-1", "rateFF":"-1"} + self.write_config(self.config) return self.config diff --git a/selfdrive/tune.py b/selfdrive/tune.py new file mode 100644 index 00000000000000..2449f083bdcd67 --- /dev/null +++ b/selfdrive/tune.py @@ -0,0 +1,157 @@ +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, 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", "reactMPC", "dampMPC", "Kp", "Ki"] + +j = 0 +while True: + print "" + print print_letters(param[j][0:9]) + print "" + print print_letters(kegman.conf[param[j]]) + print "" + print "reactMPC is an adjustment to the time projection of the MPC" + print "angle used in the dampening calculation. Increasing this value" + print "would cause the vehicle to turn sooner." + print "" + print "" + print "dampMPC is the amount of time that the samples" + print "will be projected and averaged to smooth the values" + print "" + print "" + print ("Press 1, 3, 5, 7 to incr 0.1, 0.05, 0.01, 0.001") + print ("press a, d, g, j to decr 0.1, 0.05, 0.01, 0.001") + print ("press 0 / L to make the value 0 / 1") + print ("press SPACE / m for next /prev parameter") + print ("press q to quit") + + char = getch() + write_json = False + if (char == "7"): + kegman.conf[param[j]] = str(float(kegman.conf[param[j]]) + 0.001) + write_json = True + + if (char == "5"): + kegman.conf[param[j]] = str(float(kegman.conf[param[j]]) + 0.01) + write_json = True + + elif (char == "3"): + kegman.conf[param[j]] = str(float(kegman.conf[param[j]]) + 0.05) + write_json = True + + elif (char == "1"): + kegman.conf[param[j]] = str(float(kegman.conf[param[j]]) + 0.1) + write_json = True + + elif (char == "j"): + kegman.conf[param[j]] = str(float(kegman.conf[param[j]]) - 0.001) + write_json = True + + elif (char == "g"): + kegman.conf[param[j]] = str(float(kegman.conf[param[j]]) - 0.01) + write_json = True + + elif (char == "d"): + kegman.conf[param[j]] = str(float(kegman.conf[param[j]]) - 0.05) + write_json = True + + elif (char == "a"): + kegman.conf[param[j]] = str(float(kegman.conf[param[j]]) - 0.1) + write_json = True + + elif (char == "0"): + kegman.conf[param[j]] = "0" + write_json = True + + elif (char == "l"): + kegman.conf[param[j]] = "1" + write_json = True + + 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 + + + if float(kegman.conf['tuneGernby']) != 1 and float(kegman.conf['tuneGernby']) != 0: + kegman.conf['tuneGernby'] = "1" + + if float(kegman.conf['dampMPC']) < 0 and float(kegman.conf['dampMPC']) != -1: + kegman.conf['dampMPC'] = "0" + + if float(kegman.conf['dampMPC']) > 1.0: + kegman.conf['dampMPC'] = "1.0" + + if float(kegman.conf['reactMPC']) < -0.99 and float(kegman.conf['reactMPC']) != -1: + kegman.conf['reactMPC'] = "-0.99" + + if float(kegman.conf['reactMPC']) > 1.0: + kegman.conf['reactMPC'] = "1.0" + + if float(kegman.conf['Ki']) < 0 and float(kegman.conf['Ki']) != -1: + kegman.conf['Ki'] = "0" + + if float(kegman.conf['Ki']) > 2: + kegman.conf['Ki'] = "2" + + if float(kegman.conf['Kp']) < 0 and float(kegman.conf['Kp']) != -1: + kegman.conf['Kp'] = "0" + + if float(kegman.conf['Kp']) > 3: + kegman.conf['Kp'] = "3" + + + + + + if write_json: + kegman.write_config(kegman.conf) + + time.sleep(button_delay) diff --git a/tune.py b/tune.py new file mode 100644 index 00000000000000..f4d1d0165b8fa0 --- /dev/null +++ b/tune.py @@ -0,0 +1,167 @@ +from selfdrive.kegman_conf import kegman_conf +import subprocess +import os +BASEDIR = os.path.abspath(os.path.join(os.path.dirname(os.path.realpath(__file__)), "../")) + +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", "Kp", "Ki"] + +j = 0 +while True: + print "" + print print_letters(param[j][0:9]) + print "" + print print_letters(kegman.conf[param[j]]) + print "" + print ("1, 3, 5, 7 to incr 0.1, 0.05, 0.01, 0.001") + print ("a, d, g, j to decr 0.1, 0.05, 0.01, 0.001") + print ("0 / L to make the value 0 / 1") + print ("press SPACE / m for next /prev parameter") + print ("press z to quit") + + char = getch() + write_json = False + if (char == "7"): + kegman.conf[param[j]] = str(float(kegman.conf[param[j]]) + 0.001) + write_json = True + + if (char == "5"): + kegman.conf[param[j]] = str(float(kegman.conf[param[j]]) + 0.01) + write_json = True + + elif (char == "3"): + kegman.conf[param[j]] = str(float(kegman.conf[param[j]]) + 0.05) + write_json = True + + elif (char == "1"): + kegman.conf[param[j]] = str(float(kegman.conf[param[j]]) + 0.1) + write_json = True + + elif (char == "j"): + kegman.conf[param[j]] = str(float(kegman.conf[param[j]]) - 0.001) + write_json = True + + elif (char == "g"): + kegman.conf[param[j]] = str(float(kegman.conf[param[j]]) - 0.01) + write_json = True + + elif (char == "d"): + kegman.conf[param[j]] = str(float(kegman.conf[param[j]]) - 0.05) + write_json = True + + elif (char == "a"): + kegman.conf[param[j]] = str(float(kegman.conf[param[j]]) - 0.1) + write_json = True + + elif (char == "0"): + kegman.conf[param[j]] = "0" + write_json = True + + elif (char == "l"): + kegman.conf[param[j]] = "1" + write_json = True + + 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 == "z"): + process.kill() + break + + + if float(kegman.conf['tuneGernby']) != 1 and float(kegman.conf['tuneGernby']) != 0: + kegman.conf['tuneGernby'] = "1" + + if float(kegman.conf['dampSteer']) < 0 and float(kegman.conf['dampSteer']) != -1: + kegman.conf['dampSteer'] = "0" + + if float(kegman.conf['dampMPC']) < 0 and float(kegman.conf['dampMPC']) != -1: + kegman.conf['dampMPC'] = "0" + + if float(kegman.conf['dampMPC']) > 0.3: + kegman.conf['dampMPC'] = "0.3" + + if float(kegman.conf['dampSteer']) > 1.0: + kegman.conf['dampSteer'] = "1.0" + + if float(kegman.conf['reactMPC']) < -0.99 and float(kegman.conf['reactMPC']) != -1: + kegman.conf['reactMPC'] = "-0.99" + + if float(kegman.conf['reactMPC']) > 0.1: + kegman.conf['reactMPC'] = "0.1" + + if float(kegman.conf['rateFF']) <= 0.0: + kegman.conf['rateFF'] = "0.001" + + if float(kegman.conf['reactSteer']) < -0.99 and float(kegman.conf['reactSteer']) != -1: + kegman.conf['reactSteer'] = "-0.99" + + if float(kegman.conf['reactSteer']) > 1.0: + kegman.conf['reactSteer'] = "1.0" + + if float(kegman.conf['Ki']) < 0 and float(kegman.conf['Ki']) != -1: + kegman.conf['Ki'] = "0" + + if float(kegman.conf['Ki']) > 2: + kegman.conf['Ki'] = "2" + + if float(kegman.conf['Kp']) < 0 and float(kegman.conf['Kp']) != -1: + kegman.conf['Kp'] = "0" + + if float(kegman.conf['Kp']) > 3: + kegman.conf['Kp'] = "3" + + + if write_json: + kegman.write_config(kegman.conf) + + time.sleep(button_delay) + +else: + process.kill() diff --git a/tune.sh b/tune.sh new file mode 100755 index 00000000000000..d13626c182b26c --- /dev/null +++ b/tune.sh @@ -0,0 +1 @@ +python2.7 tune.py