Completed controller

.gitignore

@@ -1,3 +1,164 @@
-.DS_Store
-.vscode/
-__pycache__/
+# Byte-compiled / optimized / DLL files
+__pycache__/
+*.py[cod]
+*$py.class
+
+# C extensions
+*.so
+
+# Distribution / packaging
+.Python
+build/
+develop-eggs/
+dist/
+downloads/
+eggs/
+.eggs/
+lib/
+lib64/
+parts/
+sdist/
+var/
+wheels/
+share/python-wheels/
+*.egg-info/
+.installed.cfg
+*.egg
+MANIFEST
+
+# PyInstaller
+#  Usually these files are written by a python script from a template
+#  before PyInstaller builds the exe, so as to inject date/other infos into it.
+*.manifest
+*.spec
+
+# Installer logs
+pip-log.txt
+pip-delete-this-directory.txt
+
+# Unit test / coverage reports
+htmlcov/
+.tox/
+.nox/
+.coverage
+.coverage.*
+.cache
+nosetests.xml
+coverage.xml
+*.cover
+*.py,cover
+.hypothesis/
+.pytest_cache/
+cover/
+
+# Translations
+*.mo
+*.pot
+
+# Django stuff:
+*.log
+local_settings.py
+db.sqlite3
+db.sqlite3-journal
+
+# Flask stuff:
+instance/
+.webassets-cache
+
+# Scrapy stuff:
+.scrapy
+
+# Sphinx documentation
+docs/_build/
+
+# PyBuilder
+.pybuilder/
+target/
+
+# Jupyter Notebook
+.ipynb_checkpoints
+
+# IPython
+profile_default/
+ipython_config.py
+
+# pyenv
+#   For a library or package, you might want to ignore these files since the code is
+#   intended to run in multiple environments; otherwise, check them in:
+# .python-version
+
+# pipenv
+#   According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control.
+#   However, in case of collaboration, if having platform-specific dependencies or dependencies
+#   having no cross-platform support, pipenv may install dependencies that don't work, or not
+#   install all needed dependencies.
+#Pipfile.lock
+
+# poetry
+#   Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control.
+#   This is especially recommended for binary packages to ensure reproducibility, and is more
+#   commonly ignored for libraries.
+#   https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control
+#poetry.lock
+
+# pdm
+#   Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control.
+#pdm.lock
+#   pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it
+#   in version control.
+#   https://pdm.fming.dev/latest/usage/project/#working-with-version-control
+.pdm.toml
+.pdm-python
+.pdm-build/
+
+# PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm
+__pypackages__/
+
+# Celery stuff
+celerybeat-schedule
+celerybeat.pid
+
+# SageMath parsed files
+*.sage.py
+
+# Environments
+.env
+.venv
+env/
+venv/
+ENV/
+env.bak/
+venv.bak/
+
+# Spyder project settings
+.spyderproject
+.spyproject
+
+# Rope project settings
+.ropeproject
+
+# mkdocs documentation
+/site
+
+# mypy
+.mypy_cache/
+.dmypy.json
+dmypy.json
+
+# Pyre type checker
+.pyre/
+
+# pytype static type analyzer
+.pytype/
+
+# Cython debug symbols
+cython_debug/
+
+# PyCharm
+#  JetBrains specific template is maintained in a separate JetBrains.gitignore that can
+#  be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore
+#  and can be added to the global gitignore or merged into this file.  For a more nuclear
+#  option (not recommended) you can uncomment the following to ignore the entire idea folder.
+#.idea/
+
+.vscode/

uuv_mission/__init__.py

@@ -0,0 +1,4 @@
+from .dynamic import Submarine
+from .dynamic import Mission
+from .dynamic import ClosedLoop
+from .control import PD_Controller

uuv_mission/control.py

@@ -0,0 +1,7 @@
+class PD_Controller:
+    def __init__(self, Kp, Kd):
+        self.proportional_gain = Kp
+        self.derivative_gain = Kd
+
+    def get_action(self, error, error_dot):
+        return self.proportional_gain * error + self.derivative_gain * error_dot

uuv_mission/dynamic.py

@@ -1,24 +1,25 @@
-from __future__ import annotations
+# from __future__ import annotations
 from dataclasses import dataclass
 import numpy as np
+import pandas as pd
 import matplotlib.pyplot as plt
 from .terrain import generate_reference_and_limits
 
+
 class Submarine:
     def __init__(self):
 
         self.mass = 1
         self.drag = 0.1
         self.actuator_gain = 1
 
-        self.dt = 1 # Time step for discrete time simulation
+        self.dt = 1  # Time step for discrete time simulation
 
         self.pos_x = 0
         self.pos_y = 0
-        self.vel_x = 1 # Constant velocity in x direction
+        self.vel_x = 1  # Constant velocity in x direction
         self.vel_y = 0
 
-
     def transition(self, action: float, disturbance: float):
         self.pos_x += self.vel_x * self.dt
         self.pos_y += self.vel_y * self.dt
@@ -29,38 +30,16 @@ def transition(self, action: float, disturbance: float):
 
     def get_depth(self) -> float:
         return self.pos_y
-    
+
     def get_position(self) -> tuple:
         return self.pos_x, self.pos_y
-    
+
     def reset_state(self):
         self.pos_x = 0
         self.pos_y = 0
         self.vel_x = 1
         self.vel_y = 0
-
-class Trajectory:
-    def __init__(self, position: np.ndarray):
-        self.position = position  
-
-    def plot(self):
-        plt.plot(self.position[:, 0], self.position[:, 1])
-        plt.show()
 
-    def plot_completed_mission(self, mission: Mission):
-        x_values = np.arange(len(mission.reference))
-        min_depth = np.min(mission.cave_depth)
-        max_height = np.max(mission.cave_height)
-
-        plt.fill_between(x_values, mission.cave_height, mission.cave_depth, color='blue', alpha=0.3)
-        plt.fill_between(x_values, mission.cave_depth, min_depth*np.ones(len(x_values)), 
-                         color='saddlebrown', alpha=0.3)
-        plt.fill_between(x_values, max_height*np.ones(len(x_values)), mission.cave_height, 
-                         color='saddlebrown', alpha=0.3)
-        plt.plot(self.position[:, 0], self.position[:, 1], label='Trajectory')
-        plt.plot(mission.reference, 'r', linestyle='--', label='Reference')
-        plt.legend(loc='upper right')
-        plt.show()
 
 @dataclass
 class Mission:
@@ -70,38 +49,95 @@ class Mission:
 
     @classmethod
     def random_mission(cls, duration: int, scale: float):
-        (reference, cave_height, cave_depth) = generate_reference_and_limits(duration, scale)
+        (reference, cave_height, cave_depth) = generate_reference_and_limits(
+            duration, scale
+        )
         return cls(reference, cave_height, cave_depth)
 
     @classmethod
     def from_csv(cls, file_name: str):
-        # You are required to implement this method
-        pass
+        data = pd.read_csv(file_name, header=0)
+        npData = data.to_numpy()
+        reference = npData[:, 0].astype(float)
+        cave_height = npData[:, 1].astype(float)
+        cave_depth = npData[:, 2].astype(float)
+
+        return cls(reference, cave_height, cave_depth)
+
+
+class Trajectory:
+    def __init__(self, position: np.ndarray):
+        self.position = position
+
+    def plot(self):
+        plt.plot(self.position[:, 0], self.position[:, 1])
+        plt.show()
+
+    def plot_completed_mission(self, mission: Mission):
+        x_values = np.arange(len(mission.reference))
+        min_depth = np.min(mission.cave_depth)
+        max_height = np.max(mission.cave_height)
+
+        plt.fill_between(
+            x_values, mission.cave_height, mission.cave_depth, color="blue", alpha=0.3
+        )
+        plt.fill_between(
+            x_values,
+            mission.cave_depth,
+            min_depth * np.ones(len(x_values)),
+            color="saddlebrown",
+            alpha=0.3,
+        )
+        plt.fill_between(
+            x_values,
+            max_height * np.ones(len(x_values)),
+            mission.cave_height,
+            color="saddlebrown",
+            alpha=0.3,
+        )
+        plt.plot(self.position[:, 0], self.position[:, 1], label="Trajectory")
+        plt.plot(mission.reference, "r", linestyle="--", label="Reference")
+        plt.legend(loc="upper right")
+        plt.show()
 
 
 class ClosedLoop:
     def __init__(self, plant: Submarine, controller):
         self.plant = plant
         self.controller = controller
 
-    def simulate(self,  mission: Mission, disturbances: np.ndarray) -> Trajectory:
+    def simulate(self, mission: Mission, disturbances: np.ndarray) -> Trajectory:
 
         T = len(mission.reference)
         if len(disturbances) < T:
-            raise ValueError("Disturbances must be at least as long as mission duration")
-
+            raise ValueError(
+                "Disturbances must be at least as long as mission duration"
+            )
+
         positions = np.zeros((T, 2))
         actions = np.zeros(T)
+        errors = np.zeros(T)
         self.plant.reset_state()
 
         for t in range(T):
+            # getting the current position
             positions[t] = self.plant.get_position()
-            observation_t = self.plant.get_depth()
-            # Call your controller here
+
+            # calculating the error and error_dot
+            errors[t] = mission.reference[t] - self.plant.get_depth()
+            error = errors[t]
+            error_dot = 0 if t == 0 else errors[t] - errors[t - 1]
+
+            # calling the controller to get the required action
+            actions[t] = self.controller.get_action(error, error_dot)
+
+            # applying the action to the plant
             self.plant.transition(actions[t], disturbances[t])
 
         return Trajectory(positions)
-
-    def simulate_with_random_disturbances(self, mission: Mission, variance: float = 0.5) -> Trajectory:
+
+    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)