Add PD Controller and Depth Simulation for UUV

.gitignore

@@ -1,3 +1,8 @@
 .DS_Store
 .vscode/
-__pycache__/
+<<<<<<< HEAD
+__pycache__/
+venv/
+=======
+__pycache__/
+>>>>>>> d423aa10851a341bb416191f14724147c4bfba0a

uuv_mission/control.py

@@ -0,0 +1,28 @@
+class PDController:
+    def __init__(self, kp, kd):
+        """
+        Initialize the PD controller.
+        :param kp: Proportional gain.
+        :param kd: Derivative gain.
+        """
+        self.kp = kp  # Proportional gain
+        self.kd = kd  # Derivative gain
+        self.prev_error = 0  # Store previous error for derivative calculation
+
+
+    def get_control_action(self, error):
+        """
+        Calculate the control action using the PD formula.
+        :param error: The current error (difference between reference and actual depth).
+        :return: Control action to adjust the submarine's depth.
+        """
+        # Derivative of error
+        derivative = error - self.prev_error
+
+        # PD control law: u[t] = kp * e[t] + kd * (e[t] - e[t-1])
+        control_action = self.kp * error + self.kd * derivative
+
+        # Update previous error for next step
+        self.prev_error = error
+
+        return control_action

uuv_mission/dynamic.py

@@ -3,6 +3,10 @@
 import numpy as np
 import matplotlib.pyplot as plt
 from .terrain import generate_reference_and_limits
+<<<<<<< HEAD
+import pandas as pd
+=======
+>>>>>>> d423aa10851a341bb416191f14724147c4bfba0a
 
 class Submarine:
     def __init__(self):
@@ -76,7 +80,20 @@ def random_mission(cls, duration: int, scale: float):
     @classmethod
     def from_csv(cls, file_name: str):
         # You are required to implement this method
+<<<<<<< HEAD
+        # Read the CSV file into a pandas DataFrame
+        df = pd.read_csv(file_name)
+
+        # Extract the relevant columns: 'reference', 'cave_height', and 'cave_depth'
+        reference = df['reference'].values
+        cave_height = df['cave_height'].values
+        cave_depth = df['cave_depth'].values
+
+        # Create and return a Mission instance
+        return cls(reference, cave_height, cave_depth)
+=======
         pass
+>>>>>>> d423aa10851a341bb416191f14724147c4bfba0a
 
 
 class ClosedLoop:
@@ -98,6 +115,16 @@ def simulate(self,  mission: Mission, disturbances: np.ndarray) -> Trajectory:
             positions[t] = self.plant.get_position()
             observation_t = self.plant.get_depth()
             # Call your controller here
+<<<<<<< HEAD
+            # Step 1: Compute the error between the reference depth and the observed depth
+            error = mission.reference[t] - observation_t
+
+            # Step 2: Use the controller to compute the control action
+            actions[t] = self.controller.get_control_action(error)
+
+            # Step 3: Apply the control action and the disturbance to the submarine
+=======
+>>>>>>> d423aa10851a341bb416191f14724147c4bfba0a
             self.plant.transition(actions[t], disturbances[t])
 
         return Trajectory(positions)