PID CONTROL STEERING WORKS!!!!!!!!!!

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

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:

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"])
 

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()