Created venv, created control module, revised simulate, revised demo,…

.gitignore

@@ -1,3 +1,5 @@
 .DS_Store
 .vscode/
-__pycache__/
+__pycache__/
+
+b1-practical-env-1

uuv_mission/control.py

@@ -0,0 +1,16 @@
+class Controller:
+    def __init__(self, Kp: float, Kd: float):
+        # Proportional and Derivative gains for the controller
+        self.Kp = Kp
+        self.Kd = Kd
+        self.previous_error = 0  # For storing the error at the previous timestep
+
+    def control(self, reference: float, observation: float) -> float:
+        # Calculate error
+        error = reference - observation
+        # PD control formula: u[t] = Kp * e[t] + Kd * (e[t] - e[t-1])
+        derivative = error - self.previous_error
+        control_action = self.Kp * error + self.Kd * derivative
+        # Store the current error as previous error for the next timestep
+        self.previous_error = error
+        return control_action

uuv_mission/dynamic.py

@@ -3,6 +3,7 @@
 import numpy as np
 import matplotlib.pyplot as plt
 from .terrain import generate_reference_and_limits
+import csv
 
 class Submarine:
     def __init__(self):
@@ -75,12 +76,18 @@ def random_mission(cls, duration: int, scale: float):
 
     @classmethod
     def from_csv(cls, file_name: str):
-        # You are required to implement this method
-        pass
+        reference, cave_height, cave_depth = [], [], []
+        with open(file_name, newline='') as csvfile:
+            reader = csv.DictReader(csvfile)
+            for row in reader:
+                reference.append(float(row['reference']))
+                cave_height.append(float(row['cave_height']))
+                cave_depth.append(float(row['cave_depth']))
+        return cls(np.array(reference), np.array(cave_height), np.array(cave_depth))
 
 
 class ClosedLoop:
-    def __init__(self, plant: Submarine, controller):
+    def __init__(self, plant: Submarine, controller: Controller):
         self.plant = plant
         self.controller = controller
 
@@ -96,12 +103,16 @@ def simulate(self,  mission: Mission, disturbances: np.ndarray) -> Trajectory:
 
         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])
+            observation_t = self.plant.get_depth()  # Get the current depth of the submarine
+            reference_t = mission.reference[t]     # Reference depth at time t
+            # Compute the control action using the controller
+            control_action = self.controller.control(reference_t, observation_t)
+            # Apply the control action along with the disturbance to the submarine
+            self.plant.transition(control_action, disturbances[t])
 
         return Trajectory(positions)
 
     def simulate_with_random_disturbances(self, mission: Mission, variance: float = 0.5) -> Trajectory:
         disturbances = np.random.normal(0, variance, len(mission.reference))
         return self.simulate(mission, disturbances)
+