Merge JiayiYangMission

.env

@@ -0,0 +1 @@
+PYTHONPATH=D:/3rd/B1_Sientific_coding_Jiayi_Yang/b1-coding-practical-mt24;${PYTHONPATH}

uuv_mission/__init__.py

@@ -0,0 +1,24 @@
+# from dynamic.py import classes
+from .dynamic import Submarine, Trajectory, Mission, ClosedLoop
+
+# from terrain.py import functions
+from .terrain import (
+    generate_random_multisine_timeseries,
+    generate_reference_and_limits, 
+    plot_reference_and_terrain,
+    write_mission_to_csv
+)
+
+__all__ = [
+    # from dynamic.py
+    'Submarine', 
+    'Trajectory', 
+    'Mission', 
+    'ClosedLoop',
+
+    #from terrain.py
+    'generate_random_multisine_timeseries',
+    'generate_reference_and_limits',
+    'plot_reference_and_terrain', 
+    'write_mission_to_csv'
+]

uuv_mission/control.py

@@ -0,0 +1,6 @@
+# a proportional derivative (PD) feedback controller
+
+def pd_controller(error, prev_error, Kp=0.15, Kd=0.6) -> float:
+
+    control_action = Kp * error + Kd * (error - prev_error)
+    return control_action

uuv_mission/dynamic.py

@@ -75,9 +75,12 @@ def random_mission(cls, duration: int, scale: float):
 
     @classmethod
     def from_csv(cls, file_name: str):
-        # You are required to implement this method
-        pass
-
+        # extracting data from the mission.csv
+        data = np.loadtxt(file_name, delimiter=',', skiprows=1)
+        reference = data[:, 0]
+        cave_height = data[:, 1]
+        cave_depth = data[:, 2]
+        return cls(reference, cave_height, cave_depth)
 
 class ClosedLoop:
     def __init__(self, plant: Submarine, controller):
@@ -92,13 +95,21 @@ def simulate(self,  mission: Mission, disturbances: np.ndarray) -> Trajectory:
 
         positions = np.zeros((T, 2))
         actions = np.zeros(T)
+        errors = np.zeros(T)
         self.plant.reset_state()
 
         for t in range(T):
-            positions[t] = self.plant.get_position()
+
             observation_t = self.plant.get_depth()
             # Call your controller here
-            self.plant.transition(actions[t], disturbances[t])
+            error = mission.reference[t] - observation_t
+            prev_error = errors[t-1] if t > 0 else 0
+            action = self.controller(error, prev_error)
+            self.plant.transition(action, disturbances[t])
+
+            positions[t] = self.plant.get_position()
+            actions[t] = action
+            errors[t] = error
 
         return Trajectory(positions)