diff --git a/play.py b/play.py index 46f14d6..66e4224 100644 --- a/play.py +++ b/play.py @@ -127,14 +127,9 @@ def control_mini(self, joystick): self.controller.set_value("BtnA",joystick[6]) self.controller.set_value("BtnX",joystick[7]) - def control_racing(self, joystick): - if type(joystick) is not float: - self.controller.set_value("AxisLx", joystick) - return + def control_racing(self, joystick: list): self.controller.set_value("AxisLx",joystick[0]) - if len(joystick) == 2: - self.controller.set_value("TriggerL",joystick[1]) - # self.controller.set_value("TriggerR", 0.5) + self.controller.set_value("TriggerR",joystick[1]) def control_throttle(self, throttle): manual_override = self.real_controller.RightThumb == 1 diff --git a/record.py b/record.py index 7d028b6..9ebd3c8 100644 --- a/record.py +++ b/record.py @@ -15,6 +15,9 @@ import sys +sys.path.append("forza_motorsport/") +from data2file import return_vals # TODO cleanup w/ telemetry.py + PY3_OR_LATER = sys.version_info[0] >= 3 if PY3_OR_LATER: @@ -51,7 +54,7 @@ def __init__(self): self.controller = XboxController() # Init telemetry - self.telemetry = get_waypoints() + self.telemetry = return_vals(PORT_NUMBER) # default args # Create GUI self.create_main_panel() @@ -124,10 +127,18 @@ def poll(self): self.update_plot() if self.recording == True: + try: + self.telem_data = next(self.telemetry) + # print(f"[DEBUG] telem data: {self.telem_data}") + except Exception as err: + # telem data is empty because currently not streaming + print(f"not getting telem data... ({err})") + self.telem_data = [] + + self.save_data() self.t += 1 - self.telem_data = next(self.telemetry) def update_plot(self): self.plotData.append(self.controller_data) # adds to the end of the list @@ -189,6 +200,8 @@ def on_btn_record(self): self.t = 0 # Reset our counter for the new recording self.record_button["text"] = "Stop" self.rate = self.sample_rate + # make telemetry (generator) + # self.telemetry = get_waypoints() # make / open outfile self.outfile = open(self.outputDir+'/'+'data.csv', 'a') else: diff --git a/segmented_driving.py b/segmented_driving.py index ee526c0..e6dca0b 100644 --- a/segmented_driving.py +++ b/segmented_driving.py @@ -128,7 +128,7 @@ def live_plot(): if __name__ == "__main__": - live_plot() + # live_plot() plot_variables(["steer", "accel", "brake", "speed", "norm_driving_line"]) diff --git a/telemetry.py b/telemetry.py index 73a3c40..1ea50e1 100644 --- a/telemetry.py +++ b/telemetry.py @@ -1,4 +1,5 @@ import sys +from typing import Any sys.path.append("forza_motorsport/") import numpy as np @@ -53,6 +54,52 @@ def pid_throttle(): print("Stopping due to Ctrl C Event") break +class pid_steer_throttle(): + MPH_TO_MPS = 0.44704 + + def __init__(self): + self.SPEED = 20 * self.MPH_TO_MPS # m/s + self.throttle_pid = PID(1, 1, 1, setpoint=self.SPEED) + self.throttle_pid.output_limits = (0, 1) + self.speed = 0 + + self.SETPOINT = 0 # center of the road + self.S_KP = 0.5 / 127.0 + self.S_KI = 0.00001 + self.S_KD = 0.25 + self.steer_pid = PID(self.S_KP, self.S_KI, self.S_KD, setpoint=self.SETPOINT) + self.steer_pid.output_limits = (-1, 1) + self.norm_driving_line = 0 + + self.generator = return_vals(PORT_NUMBER) # norm_driving_line, speed + + self.actor = Actor(load_model=False) + + def __update(self): + data = next(self.generator) + self.speed = data[-1] + self.norm_driving_line = data[0] + assert type(self.speed) is float and type(self.norm_driving_line) in (float, int), f"Error (Assert): {self.speed}, {self.norm_driving_line}" + + steer = - self.steer_pid(self.norm_driving_line) + throttle = self.throttle_pid(self.speed) + + # debug - print current steer, norm_driving_line + print(f"Steer: {steer}, Norm Driving Line: {self.norm_driving_line}, data: {data}") + + return steer, throttle + + def __call__(self) -> Any: + while True: + try: + steer, throttle = self.__update() + self.actor.control_racing([steer, throttle]) + except KeyboardInterrupt: + print("Stopping due to Ctrl C Event") + break + # except Exception as e: + # print(f"Error (Loop): {e}") + def follow_waypoints(waypoints_file="waypoints.txt"): actor = Actor(load_model=False) @@ -90,10 +137,11 @@ def follow_waypoints(waypoints_file="waypoints.txt"): if __name__ == "__main__": # save_waypoints() # pid_throttle() + pid_steer_throttle()() # this is so funny #for i in get_waypoints(print_values=False): # x, y, z, speed = i # print(f"x: {x}, y: {y}, z: {z}, speed: {speed}") # # time.sleep(1) - follow_waypoints() + # follow_waypoints()