From a601354250988071e829959ec38bffa3142967ea Mon Sep 17 00:00:00 2001
From: ZegesMenden <51548005+ZegesMenden@users.noreply.github.com>
Date: Tue, 3 May 2022 15:00:32 -0400
Subject: [PATCH 1/7] Update test.py

---
 test.py | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/test.py b/test.py
index f52e679..63ef706 100644
--- a/test.py
+++ b/test.py
@@ -37,8 +37,8 @@ def main():
         noise=0.001
     )
 
-    PID_y: torque_PID = torque_PID(Kp=25, Ki=0.0, Kd=15, inertia=rocket_1.body.moment_of_inertia.y, lever_arm=0.2)
-    PID_z: torque_PID = torque_PID(Kp=25, Ki=0.0, Kd=15, inertia=rocket_1.body.moment_of_inertia.z, lever_arm=0.2)
+    PID_y: torque_PID = torque_PID(Kp=25, Ki=0.0, Kd=10, inertia=rocket_1.body.moment_of_inertia.y, lever_arm=0.2)
+    PID_z: torque_PID = torque_PID(Kp=25, Ki=0.0, Kd=10, inertia=rocket_1.body.moment_of_inertia.z, lever_arm=0.2)
 
     @tvc_1.update_func
     def tvc1_update():
@@ -46,14 +46,14 @@ def tvc1_update():
         target_vector: Vec3 = Vec3(1.0, 0.0, 0.0)
 
         if simulation.time > 0.5:
-            target_vector.y += 0.3
+            target_vector.y += 0.1
 
         target_vector = rocket_1.body.rotation.conjugate().rotate(target_vector.normalize())
 
         rotVel = rocket_1.body.rotation.conjugate().rotate(rocket_1.body.rotational_velocity)
 
-        PID_y.update(np.arctan2(-target_vector.z, target_vector.x), tvc_1.update_delay, rocket_1.motor_thrust, rotVel.y)
-        PID_z.update(np.arctan2(target_vector.y, target_vector.x), tvc_1.update_delay, rocket_1.motor_thrust, rotVel.z)
+        PID_y.update(np.arctan2(-target_vector.z, target_vector.x) * PID_z.getOutput()*(180/np.pi), tvc_1.update_delay, rocket_1.motor_thrust, rotVel.y)
+        PID_z.update(np.arctan2(target_vector.y, target_vector.x) * PID_z.getOutput()*(180/np.pi), tvc_1.update_delay, rocket_1.motor_thrust, rotVel.z)
 
         return Vec3(0.0, PID_y.getOutput()*(180/np.pi), PID_z.getOutput()*(180/np.pi))
 

From f98d216684b75ea680cbdbe1bb4bdb5bb1505d09 Mon Sep 17 00:00:00 2001
From: ZegesMenden <51548005+ZegesMenden@users.noreply.github.com>
Date: Wed, 4 May 2022 19:05:37 -0400
Subject: [PATCH 2/7] added custom data points (not working)

---
 pytvc/simulation.py | 119 ++++++++++++++++++++++++++++++--------------
 1 file changed, 83 insertions(+), 36 deletions(-)

diff --git a/pytvc/simulation.py b/pytvc/simulation.py
index c040d46..16e2861 100644
--- a/pytvc/simulation.py
+++ b/pytvc/simulation.py
@@ -5,6 +5,7 @@
 
 RAD2DEG = 180.0/np.pi
 
+
 class rocket:
 
     def __init__(self, dry_mass: float = 1.0, time_step: float = 0.001, name: str = "", log_values: bool = False, log_frequency: int = 50, print_info: bool = False, do_aero: bool = True, drag_coeff_forewards: float = 0.0, drag_coeff_sideways: float = 0.0, refence_area: float = 0.0, cp_location: Vec3 = Vec3(), moment_of_inertia: Vec3 = Vec3(1.0, 1.0, 1.0), friction_coeff: float = 0.0) -> None:
@@ -59,6 +60,9 @@ def __init__(self, dry_mass: float = 1.0, time_step: float = 0.001, name: str =
             friction_coeff=friction_coeff
         )
 
+        # user defined data points
+        self._data_points = []
+
         # function registry for update and setup
         self._function_registry = {}
 
@@ -69,7 +73,16 @@ def setup(self, func):
     def update(self, func):
         self._function_registry["update"] = func
         return func
-    
+
+    def add_data_point(self, name: str, func: callable):
+        """add_data_point add a data point to the rocket's datalogger
+
+        Args:
+            name (str): name of the data
+            func (callable): function to call to get the data
+        """
+        self._data_points.append({"name": name, "func": func})
+
     def add_tvc_mount(self, tvc_mount: TVC, position: Vec3) -> None:
         """adds a TVC mount to the rocket
 
@@ -82,16 +95,18 @@ def add_tvc_mount(self, tvc_mount: TVC, position: Vec3) -> None:
             Exception: _description_
             Exception: _description_
         """
-        
+
         if isinstance(tvc_mount, TVC):
             self._n_tvc_mounts += 1
             if tvc_mount.name == "":
-                tvc_mount.name = f"TVC{self._n_tvc_mounts}" 
-            
+                tvc_mount.name = f"TVC{self._n_tvc_mounts}"
+
             if tvc_mount.name in self._tvc_mounts:
-                raise Exception(f"TVC mount with name {tvc_mount.name} already exists")
+                raise Exception(
+                    f"TVC mount with name {tvc_mount.name} already exists")
             else:
-                self._tvc_mounts[tvc_mount.name] = {"mount": tvc_mount, "position": position}
+                self._tvc_mounts[tvc_mount.name] = {
+                    "mount": tvc_mount, "position": position}
         else:
             raise Exception("TVC mount must be of type TVC")
 
@@ -100,11 +115,12 @@ def add_parachute(self, chute: parachute, position: Vec3):
         if isinstance(chute, parachute):
             self._n_parachutes += 1
             if chute.name == "":
-                chute.name = f"parachute_{self._n_parachutes}" 
-            self._parachutes[chute.name] = {"parachute": chute, "position": position}
+                chute.name = f"parachute_{self._n_parachutes}"
+            self._parachutes[chute.name] = {
+                "parachute": chute, "position": position}
         else:
             raise Exception("Parachute must be of type parachute")
-    
+
     def _initialize(self) -> None:
         if self.log_values:
             if self.print_info:
@@ -140,7 +156,8 @@ def _initialize(self) -> None:
                 if isinstance(t, TVC):
                     if t.log_data:
                         if self.print_info:
-                            print(f"    {self.name}: initializing TVC mount {t.name} data logger")
+                            print(
+                                f"    {self.name}: initializing TVC mount {t.name} data logger")
                         self.datalogger.add_datapoint(f"{t.name}_position_y")
                         self.datalogger.add_datapoint(f"{t.name}_position_z")
                         self.datalogger.add_datapoint(f"{t.name}_throttle")
@@ -149,17 +166,24 @@ def _initialize(self) -> None:
                         self.datalogger.add_datapoint(f"{t.name}_error_y")
                         self.datalogger.add_datapoint(f"{t.name}_error_z")
 
+            if len(self._data_points) != 0:
+                for dp in self._data_points:
+                    if self.print_info:
+                        print(f"    {self.name}: adding data point {dp['name']}")
+                    self.datalogger.add_datapoint(dp["name"])
+            
+            self.datalogger.fileName = f"{self.name}_log.csv"
+            self.datalogger.initialize_csv(True, True)
+            
+            if self.print_info:
+                print(f"    {self.name}: data logger initialized")
+
         if "setup" in self._function_registry:
             if self.print_info:
                 print(f"    {self.name}: running user setup function")
-            self._function_registry["setup"]()
-
-        if self.log_values:        
-            self.datalogger.fileName = f"{self.name}_log.csv"
-            self.datalogger.initialize_csv(True, True)
+            self._function_registry["setup"]()           
 
     def _update(self) -> None:
-
         """update the body's simulation by one time step"""
 
         self.time += self.time_step
@@ -173,21 +197,23 @@ def _update(self) -> None:
             tm, f = self._tvc_mounts[mount]["mount"].get_values(self.time)
             tmp_m += tm*0.001
             self.motor_thrust += f.length()
-            self.body.apply_local_point_force(f, self._tvc_mounts[mount]["position"])
+            self.body.apply_local_point_force(
+                f, self._tvc_mounts[mount]["position"])
 
         if self.body.position.x > 0.01:
             for p in self._parachutes:
                 if self._parachutes[p]["parachute"]._check(self.body.position, self.body.velocity):
-                    self.body.apply_force(self._parachutes[p]["parachute"].calculate_forces(self.body.mass, self.body.velocity))#, self._parachutes[p]["position"])
+                    self.body.apply_force(self._parachutes[p]["parachute"].calculate_forces(
+                        self.body.mass, self.body.velocity))  # , self._parachutes[p]["position"])
 
         self.body.mass = self.dry_mass + tmp_m
         self.engine_mass = tmp_m
-        
+
         self.body.update(self.time_step)
 
         if self.body.position.x > self.apogee:
             self.apogee = self.body.position.x
-        
+
         if "update" in self._function_registry:
             self._function_registry["update"]()
 
@@ -222,17 +248,24 @@ def _update(self) -> None:
                     self.datalogger.record_variable(f"{t.name}_position_y", t._rotationEulers.y * RAD2DEG)
                     self.datalogger.record_variable(f"{t.name}_position_z", t._rotationEulers.z * RAD2DEG)
                     self.datalogger.record_variable(f"{t.name}_throttle", t._throttle)
-                    self.datalogger.record_variable(f"{t.name}_setpoint_y", t.targetEulers.y * RAD2DEG) 
-                    self.datalogger.record_variable(f"{t.name}_setpoint_z", t.targetEulers.z * RAD2DEG) 
+                    self.datalogger.record_variable(f"{t.name}_setpoint_y", t.targetEulers.y * RAD2DEG)
+                    self.datalogger.record_variable(f"{t.name}_setpoint_z", t.targetEulers.z * RAD2DEG)
                     self.datalogger.record_variable(f"{t.name}_error_y", 0.0)
                     self.datalogger.record_variable(f"{t.name}_error_z", 0.0)
+            
+            if len(self._data_points) != 0:
+                for dp in self._data_points:
+                    self.datalogger.record_variable(dp["name"], dp["func"]())
+            
             self.datalogger.save_data(True)
             self.last_log_time = self.time
-            
+
         self.body.clear()
 
+
 class sim:
-    
+    """ simulation class"""
+
     def __init__(self, plot_data: bool = True, time_step: float = 0.001, time_end: float = 60.0, rockets: dict = {}, print_times: bool = True, print_info: bool = True, wind: Vec3 = Vec3(), sim_wind: bool = False, random_wind: bool = False) -> None:
         self.time: float = 0.0
         self.time_step = time_step
@@ -247,9 +280,10 @@ def __init__(self, plot_data: bool = True, time_step: float = 0.001, time_end: f
         self.use_wind = sim_wind
 
         if random_wind:
-            self.wind: Vec3 = Vec3(0.0, np.random.normal(0.0, 0.5, 1)[0], np.random.normal(0.0, 0.5, 1)[0])
+            self.wind: Vec3 = Vec3(0.0, np.random.normal(0.0, 0.5, 1)[
+                                   0], np.random.normal(0.0, 0.5, 1)[0])
         else:
-            self.wind: Vec3 = wind        
+            self.wind: Vec3 = wind
 
         for r in rockets:
             if isinstance(r, rocket):
@@ -257,7 +291,7 @@ def __init__(self, plot_data: bool = True, time_step: float = 0.001, time_end: f
                     r.name = "rocket" + str(self.nRockets)
                 if sim_wind:
                     r.body.wind_speed = self.wind
-                self._rockets[r.name] = r              
+                self._rockets[r.name] = r
                 self.nRockets += 1
             else:
                 raise TypeError("Rocket must be a rocket class")
@@ -267,7 +301,7 @@ def add_rocket(self, r: rocket) -> None:
         if isinstance(r, rocket):
             if r.name == "":
                 r.name = "rocket" + str(self.nRockets)
-            self._rockets[r.name] = r              
+            self._rockets[r.name] = r
             self.nRockets += 1
         else:
             raise TypeError("Rocket must be a rocket class")
@@ -297,8 +331,9 @@ def run(self) -> None:
 
             self.time += self.time_step
             if self.time % 0.5 < self.time_step:
-                print("Running simulation " + progress_bar(self.time, self.time_end), end="\r")
-                
+                print("Running simulation " +
+                      progress_bar(self.time, self.time_end+self.time_step), end="\r")
+
             for r in self._rockets:
                 self._rockets[r]._update()
 
@@ -313,18 +348,30 @@ def run(self) -> None:
                     pTmp: plotter = plotter()
                     pTmp.read_header(f"{_rocket.name}_log.csv")
 
-                    pTmp.create_2d_graph(['time', 'position_x', 'position_y', 'position_z', 'velocity_x', 'velocity_y', 'velocity_z', 'acceleration_x_world', 'acceleration_y_world', 'acceleration_z_world'], "x", "y", True, posArg=221)
+                    pTmp.create_2d_graph(['time', 'position_x', 'position_y', 'position_z', 'velocity_x', 'velocity_y', 'velocity_z',
+                                         'acceleration_x_world', 'acceleration_y_world', 'acceleration_z_world'], "x", "y", True, posArg=221)
                     # pTmp.create_2d_graph(['time', 'velocity_x', 'velocity_y', 'velocity_z'], "x", "y", True)
-                    pTmp.create_3d_graph(['position_x', 'position_y', 'position_z'], size=_rocket.apogee, posArg=122)
+                    pTmp.create_3d_graph(
+                        ['position_x', 'position_y', 'position_z'], size=_rocket.apogee, posArg=122)
                     for tvc in _rocket._tvc_mounts:
                         t = _rocket._tvc_mounts[tvc]["mount"]
                         if isinstance(t, TVC):
-                            pTmp.create_2d_graph(['time', 'rotation_x', 'rotation_y', 'rotation_z', f'{t.name}_position_y', f'{t.name}_position_z', f'{t.name}_setpoint_y', f'{t.name}_setpoint_z'], "x", "y", True, posArg=223)
+                            pTmp.create_2d_graph(['time', 'rotation_x', 'rotation_y', 'rotation_z',
+                                                 f'{t.name}_position_y', f'{t.name}_position_z', f'{t.name}_setpoint_y', f'{t.name}_setpoint_z'], "x", "y", True, posArg=223)
                         else:
-                            pTmp.create_2d_graph(['time', 'rotation_x', 'rotation_y', 'rotation_z'], "x", "y", True)
+                            pTmp.create_2d_graph(
+                                ['time', 'rotation_x', 'rotation_y', 'rotation_z'], "x", "y", True)
                     # pTmp.create_2d_graph(['time', 'acceleration_x_world', 'acceleration_y_world', 'acceleration_z_world'], "x", "y", True)
 
-                    
+                    # if len(_rocket._data_points) != 0:
+                    #     l: list = ['time']
+                    #     for dp in _rocket._data_points:
+                    #         l.append(dp["name"])
+
+                    #     print(l)
+
+                    pTmp.create_2d_graph(['T'], "x", "y", False)
+
                     pTmp.show_all_graphs()
 
-        return None
\ No newline at end of file
+        return None

From 203e9477ec1f3b1a2165f20e12a2a28ac7d0b76b Mon Sep 17 00:00:00 2001
From: ZegesMenden <51548005+ZegesMenden@users.noreply.github.com>
Date: Wed, 4 May 2022 19:05:48 -0400
Subject: [PATCH 3/7] fixed randfom things

---
 pytvc/physics.py | 46 +++++++++++++++++++++++-----------------------
 1 file changed, 23 insertions(+), 23 deletions(-)

diff --git a/pytvc/physics.py b/pytvc/physics.py
index 488975f..13adf0c 100644
--- a/pytvc/physics.py
+++ b/pytvc/physics.py
@@ -3,6 +3,7 @@
 import numpy as np
 from pytvc.data import rocket_motor
 
+
 def clamp(x, min_val, max_val):
     return max(min(x, max_val), min_val)
 
@@ -292,7 +293,6 @@ def __truediv__(self, scalar: float) -> Quat:
 
         if isinstance(scalar, float) or isinstance(scalar, int):
             return Quat(self.w / scalar, self.x / scalar, self.y / scalar, self.z / scalar)
-        
 
     def __add__(self, other: Quat) -> Quat:
         """__add__ adds 2 Quats and returns the sum
@@ -582,7 +582,8 @@ def update(self, dt: float):
         self._rotationEulers.z = clamp(
             self._rotationEulers.z, self.minPosition.z, self.maxPosition.z)
 
-        self._rotation = Quat().from_euler(self._rotationEulers + self.offset + Vec3(0.0, np.random.normal(0.0, 1, 1)[0] * self.noise, np.random.normal(0.0, 1, 1)[0] * self.noise))
+        self._rotation = Quat().from_euler(self._rotationEulers + self.offset + Vec3(0.0,
+                                                                                     np.random.normal(0.0, 1, 1)[0] * self.noise, np.random.normal(0.0, 1, 1)[0] * self.noise))
         self._rotationEulers = self._rotation.to_euler()
         self._rotationEulers.x = 0.0
 
@@ -745,6 +746,14 @@ def apply_torque(self, torque: Vec3) -> None:
         """
         self.rotational_acceleration += (torque / self.moment_of_inertia)
 
+    def apply_local_torque(self, torque: Vec3) -> None:
+        """apply a torque in the local frame
+
+        Args:
+            torque (Vec3): the torque to apply
+        """
+        self.apply_torque(self.rotation.rotate(torque))
+
     def apply_point_torque(self, force: Vec3, point: Vec3) -> None:
         """apply a point torque in the global frame
 
@@ -755,14 +764,6 @@ def apply_point_torque(self, force: Vec3, point: Vec3) -> None:
         tmp = point.cross(force)
         self.apply_torque(tmp)
 
-    def apply_local_torque(self, torque: Vec3) -> None:
-        """apply a torque in the local frame
-
-        Args:
-            torque (Vec3): the torque to apply
-        """
-        self.apply_torque(self.rotation.rotate(torque))
-
     def apply_local_point_torque(self, force: Vec3, point: Vec3) -> None:
         """apply a point torque in the local frame
 
@@ -770,8 +771,7 @@ def apply_local_point_torque(self, force: Vec3, point: Vec3) -> None:
             force (Vec3): the force to apply 
             point (Vec3): the distance of the force from the center of mass
         """
-        self.apply_point_torque(self.rotation.rotate(
-            force), self.rotation.rotate(point))
+        self.apply_point_torque(self.rotation.rotate(force), self.rotation.rotate(point))
 
     def apply_force(self, force: Vec3) -> None:
         """apply a force on the body in the global frame
@@ -781,6 +781,14 @@ def apply_force(self, force: Vec3) -> None:
         """
         accel: Vec3 = force / self.mass
         self.acceleration += accel
+    
+    def apply_local_force(self, force: Vec3) -> None:
+        """apply a force in the local frame
+
+        Args:
+            force (Vec3): the force to apply
+        """
+        self.apply_force(self.rotation.rotate(force))
 
     def apply_point_force(self, force: Vec3, point: Vec3) -> None:
         """apply a point force in the global frame, affects rotation and translation
@@ -792,14 +800,6 @@ def apply_point_force(self, force: Vec3, point: Vec3) -> None:
         self.apply_force(force)
         self.apply_point_torque(force, point)
 
-    def apply_local_force(self, force: Vec3) -> None:
-        """apply a force in the local frame
-
-        Args:
-            force (Vec3): the force to apply
-        """
-        self.apply_force(self.rotation.rotate(force))
-
     def apply_local_point_force(self, force: Vec3, point: Vec3) -> None:
         """apply a point force in the local frame, affects rotation and translation
 
@@ -807,8 +807,7 @@ def apply_local_point_force(self, force: Vec3, point: Vec3) -> None:
             force (Vec3): the force to apply
             point (Vec3): the distance of the force from the bodies center of mass
         """
-        self.apply_local_force(self.rotation.rotate(force))
-        self.apply_local_point_torque(force, point)
+        self.apply_point_force(self.rotation.rotate(force), self.rotation.rotate(point))
 
     def update(self, dt: float) -> None:
         """updates the physics body for the given change in time
@@ -834,7 +833,8 @@ def update(self, dt: float) -> None:
                 ((-np.cos(self.aoa)/2.0 + 0.5) *
                  (self.drag_coefficient_sideways - self.drag_coefficient_forwards))
 
-            self.drag_force = -velocity_relative_wind.normalize() * (drag_coefficient/2.0 * self.air_density * self.ref_area * (self.velocity.length()*self.velocity.length()))
+            self.drag_force = -velocity_relative_wind.normalize() * (drag_coefficient/2.0 *
+                                                                     self.air_density * self.ref_area * (self.velocity.length()*self.velocity.length()))
 
             self.apply_point_force(self.drag_force, self.cp_location)
             self.apply_torque(self.rotational_velocity * -

From fccc32f169a57e60f22bc2ad22838f7b08167b45 Mon Sep 17 00:00:00 2001
From: ZegesMenden <51548005+ZegesMenden@users.noreply.github.com>
Date: Wed, 4 May 2022 19:08:55 -0400
Subject: [PATCH 4/7] fixed imports pt2

---
 pytvc/__init__.py | 6 +-----
 1 file changed, 1 insertion(+), 5 deletions(-)

diff --git a/pytvc/__init__.py b/pytvc/__init__.py
index f928054..e9c6db9 100644
--- a/pytvc/__init__.py
+++ b/pytvc/__init__.py
@@ -1,9 +1,5 @@
-# external libraries
-import numpy as np
-
-# internal libraries
 import pytvc.control
 import pytvc.simulation
 import pytvc.data
 import pytvc.physics
-from pytvc.physics import Vec3, Quat
\ No newline at end of file
+from pytvc.physics import Vec3, Quat

From a1af3c7b130749c0cd153af9407189f26e22b61e Mon Sep 17 00:00:00 2001
From: ZegesMenden <51548005+ZegesMenden@users.noreply.github.com>
Date: Wed, 4 May 2022 19:09:04 -0400
Subject: [PATCH 5/7] its all broken

---
 pytvc/control.py | 31 +++++++++++++++++--------------
 1 file changed, 17 insertions(+), 14 deletions(-)

diff --git a/pytvc/control.py b/pytvc/control.py
index c056846..3ecab3a 100644
--- a/pytvc/control.py
+++ b/pytvc/control.py
@@ -1,10 +1,11 @@
 from pytvc.physics import Vec3
 import numpy as np
 
+
 class PID:
 
     """PID controller class
-    
+
     for more information on the PID controller, see:
     https://en.wikipedia.org/wiki/PID_controller"""
 
@@ -27,7 +28,7 @@ def __init__(self, Kp: float = 0.0, Ki: float = 0.0, Kd: float = 0.0, setpoint:
         self.last_error: float = 0.0
 
         self.output: float = 0.0
-    
+
     def update(self, input: float, dt: float = 1.0, input_derivitive: float = 0.0) -> None:
         error = self.setpoint - input
 
@@ -35,19 +36,19 @@ def update(self, input: float, dt: float = 1.0, input_derivitive: float = 0.0) -
             self.i = self.i_max
         elif self.i < -self.i_max:
             self.i = -self.i_max
-        
+
         d = (error - self.last_error) / dt
         self.last_error = error
 
         if input_derivitive != 0.0:
             d = input_derivitive
-        
-        self.output = self.Kp * error + self.Ki * self.i + self.Kd * d
+
+        self.output = (self.Kp * error) + (self.Ki * self.i) + (self.Kd * d)
 
     def reset(self) -> None:
         self.i = 0.0
         self.last_error = 0.0
-    
+
     def setSetpoint(self, setpoint: float) -> None:
         """setSetpoint sets the setpoint of the PID controller
 
@@ -55,7 +56,7 @@ def setSetpoint(self, setpoint: float) -> None:
             setpoint (float): setpoint of the PID controller
         """
         self.setpoint = setpoint
-    
+
     def setKp(self, Kp: float) -> None:
         """setKp sets the proportional gain of the PID controller
 
@@ -63,7 +64,7 @@ def setKp(self, Kp: float) -> None:
             Kp (float): proportional gain of the PID controller
         """
         self.Kp = Kp
-    
+
     def setKi(self, Ki: float) -> None:
         """setKi sets the integral gain of the PID controller
 
@@ -71,7 +72,7 @@ def setKi(self, Ki: float) -> None:
             Ki (float): integral gain of the PID controller
         """
         self.Ki = Ki
-    
+
     def setKd(self, Kd: float) -> None:
         """setKd sets the derivative gain of the PID controller
 
@@ -79,7 +80,7 @@ def setKd(self, Kd: float) -> None:
             Kd (float): derivative gain of the PID controller
         """
         self.Kd = Kd
-    
+
     def setImax(self, i_max: float) -> None:
         """setImax set the maximum integral value for the PID controller
 
@@ -87,7 +88,7 @@ def setImax(self, i_max: float) -> None:
             i_max (float): maximum value for the integral
         """
         self.i_max = i_max
-    
+
     def getOutput(self) -> float:
         """getOutput returns the output of the PID controller
 
@@ -96,6 +97,7 @@ def getOutput(self) -> float:
         """
         return self.output
 
+
 class torque_PID(PID):
 
     def __init__(self, Kp: float = 0.0, Ki: float = 0.0, Kd: float = 0.0, setpoint: float = 0.0, i_max: float = 0.0, inertia: float = 1.0, lever_arm: float = 1.0) -> None:
@@ -119,13 +121,14 @@ def update(self, input: float, dt: float = 1.0, force: float = 1.0, input_derivi
         if input_derivitive == 0.0:
             super().update(input, dt)
         else:
-            super().update(input, dt, input_derivitive)    
+            super().update(input, dt, input_derivitive)
 
         if force != 0.0:
-            calcval = super().getOutput() * self.inertia / force / self.lever_arm
+            calcval = ((super().getOutput() * self.inertia) / force) / self.lever_arm
+            print(calcval)
             if abs(calcval) > 1.0:
                 self.output = 0.0
             else:
                 self.output = np.arcsin(calcval)
         else:
-            self.output = 0.0
\ No newline at end of file
+            self.output = 0.0

From 91143a1c80843d2e0e06a0400e260ca418ea8f89 Mon Sep 17 00:00:00 2001
From: ZegesMenden <51548005+ZegesMenden@users.noreply.github.com>
Date: Wed, 4 May 2022 19:09:25 -0400
Subject: [PATCH 6/7] formatting and fixed progress bar bug

---
 pytvc/data.py | 64 +++++++++++++++++++++++++++++++++------------------
 1 file changed, 41 insertions(+), 23 deletions(-)

diff --git a/pytvc/data.py b/pytvc/data.py
index b518cbb..51b7484 100644
--- a/pytvc/data.py
+++ b/pytvc/data.py
@@ -8,16 +8,24 @@
 
 # from pytvc.physics import Vec3, Quat
 
+
 def progress_bar(n, maxn) -> str:
+    
     progress: str = ""
     end = int(n/maxn*50)
-    for i in range(50): 
-        if i > end: 
-            progress += ' ' 
-        else: 
-            progress += "="
+    
+    for i in range(50):
+        if i > end:
+            progress += ' '
+        else:
+            if i+1 > end:
+                progress += '>'
+            else:
+                progress += "="
+
     return (f"""[{progress}] Progress: {round((n/maxn)*100, 2)}%""")
 
+
 class data_logger:
     def __init__(self):
         self.variables = 0
@@ -124,6 +132,7 @@ def get_var(self, variableName):
         else:
             raise IndexError("datapoint not initialized")
 
+
 class data_visualizer:
     def __init__(self):
         self.allDataDescriptions = []
@@ -156,6 +165,7 @@ def graph_from_csv(self, datapoints, file_name):
 
         return dataOut
 
+
 class plotter:
 
     """Data visualization class"""
@@ -197,7 +207,7 @@ def create_2d_graph(self, graph_points: list, x_desc: str = "", y_desc: str = ""
         else:
             self.n_plots += 1
             plt.figure(self.n_plots)
-        
+
         plot_points = self.viewer.graph_from_csv(graph_points, self.file_name)
 
         for index, dataPoint in enumerate(plot_points):
@@ -208,7 +218,7 @@ def create_2d_graph(self, graph_points: list, x_desc: str = "", y_desc: str = ""
                 else:
                     plt.plot(plot_points[0], dataPoint)
         if annotate:
-           plt.legend()
+            plt.legend()
         plt.xlabel(x_desc)
         plt.ylabel(y_desc)
 
@@ -230,7 +240,8 @@ def create_3d_graph(self, graph_points: list, size: float = 0.0, color: str = 'B
             if self.n_plots == 0:
                 plt.figure(figsize=(16, 8))
                 self.n_plots += 1
-            plt.subplot(posArg, projection='3d', xlim=(-size, size), ylim=(-size, size), zlim=(-0, size))
+            plt.subplot(posArg, projection='3d', xlim=(-size, size),
+                        ylim=(-size, size), zlim=(-0, size))
             plt.plot(plot_points[2], plot_points[1], plot_points[0])
         else:
             self.n_plots += 1
@@ -245,16 +256,17 @@ def create_3d_graph(self, graph_points: list, size: float = 0.0, color: str = 'B
                 ax.set_zlim3d(0, size)
 
             ax.scatter3D(plot_points[2], plot_points[1],
-                        plot_points[0], c=plot_points[2], cmap=color)
+                         plot_points[0], c=plot_points[2], cmap=color)
 
     def create_3d_animation(self, graph_points: list, size: float, time: float = 5.0, color: str = 'Blues'):
-        
+
         self.n_plots += 1
         fig = plt.figure(self.n_plots)
-        
+
         ax = p3.Axes3D(fig)
 
-        plot_position = self.viewer.graph_from_csv(graph_points, self.file_name)
+        plot_position = self.viewer.graph_from_csv(
+            graph_points, self.file_name)
 
         ax.set_xlim3d(-size, size)
         ax.set_ylim3d(-size, size)
@@ -265,14 +277,15 @@ def func(num, dataSet, line):
             xcur = dataSet[2][num] - 1
             numpog = 0
             for index, x in enumerate(dataSet[2]):
-                if x >= xcur-0.1 and x <= xcur -0.1:
+                if x >= xcur-0.1 and x <= xcur - 0.1:
                     numpog = index
             line.set_data(dataSet[0:2, num-20:num])
             line.set_3d_properties(dataSet[2, num-20:num])
             return line
 
         # # THE DATA POINTS
-        t = np.array(plot_position[1]) # This would be the z-axis ('t' means time here)
+        # This would be the z-axis ('t' means time here)
+        t = np.array(plot_position[1])
         x = np.array(plot_position[2])
         y = np.array(plot_position[3])
 
@@ -280,7 +293,8 @@ def func(num, dataSet, line):
         dataSet = np.array([x, y, t])
 
         # NOTE: Can't pass empty arrays into 3d version of plot()
-        line = plt.plot(dataSet[0], dataSet[1], dataSet[2], lw=2, c='g')[0] # For line plot
+        line = plt.plot(dataSet[0], dataSet[1], dataSet[2], lw=2, c='g')[
+            0]  # For line plot
 
         ax.set_xlabel('y')
         ax.set_ylabel('z')
@@ -288,12 +302,15 @@ def func(num, dataSet, line):
         ax.set_title('Trajectory')
 
         # Creating the Animation object
-        line_ani = animation.FuncAnimation(fig, func, frames=numDataPoints, fargs=(dataSet,line), interval=(time/numDataPoints), blit=False)
+        line_ani = animation.FuncAnimation(fig, func, frames=numDataPoints, fargs=(
+            dataSet, line), interval=(time/numDataPoints), blit=False)
         plt.show()
+
     def show_all_graphs(self):
         """Displays all graphs."""
         plt.show()
 
+
 class rocket_motor:
 
     """class representing a rocket motor"""
@@ -309,18 +326,19 @@ def __init__(self, filePath: str, timeStep: int, ignitionTime: float = -1.0) ->
         self._ignitionTime: float = ignitionTime
         self._timeStep: int = timeStep
         self._data = []
-        
+
         if filePath != "":
-            
+
             tree = ET.parse(filePath)
             root = tree.getroot()
-            
+
             eng_data = root[0][0][1]
-            
+
             lPoint = [0, 0, 0]
-            
+
             for data in eng_data:
-                dataTmp = [float(data.attrib['t']), float(data.attrib['f']), float(data.attrib['m'])]
+                dataTmp = [float(data.attrib['t']), float(
+                    data.attrib['f']), float(data.attrib['m'])]
                 if dataTmp[0] > 0:
                     thrustDiff = dataTmp[1] - lPoint[1]
                     massDiff = dataTmp[2] - lPoint[2]
@@ -371,4 +389,4 @@ def update(self, time: float) -> tuple[float, float]:
             else:
                 return 0.0, self._data[-1][1]*0.001
         else:
-            return 0.0, self._data[0][1]*0.001
\ No newline at end of file
+            return 0.0, self._data[0][1]*0.001

From de0505c7f30ef9e14a0fc62d771ac41649d19f1f Mon Sep 17 00:00:00 2001
From: ZegesMenden <51548005+ZegesMenden@users.noreply.github.com>
Date: Wed, 4 May 2022 19:09:31 -0400
Subject: [PATCH 7/7] it works!

---
 test.py | 134 ++++++++++++++++++++++++++++----------------------------
 1 file changed, 68 insertions(+), 66 deletions(-)

diff --git a/test.py b/test.py
index 63ef706..f64d36b 100644
--- a/test.py
+++ b/test.py
@@ -1,92 +1,94 @@
 import pytvc
 from pytvc.simulation import rocket, sim
-from pytvc.physics import Vec3, Quat, TVC, rocket_motor, parachute
+from pytvc.physics import Vec3, Quat, TVC, rocket_motor, parachute, physics_body
 from pytvc.control import PID, torque_PID
 import numpy as np
 
-def main():
+# create the simulation object
+simulation: sim = sim(
+    time_end=15.0,
+    sim_wind=True,
+    random_wind=True
+)
 
-    # create the simulation object
-    simulation: sim = sim(
-        time_end=15.0,
-        sim_wind=True,
-        random_wind=True
-    )
+# create our first rocket
+rocket_1: rocket = rocket(
+    name="rocket_1",
+    log_values=True,
+    do_aero=True,
+    dry_mass=0.4,
+    refence_area=np.pi*(0.076/2)**2,
+    drag_coeff_forewards=1.35,
+    drag_coeff_sideways=7,
+    cp_location=Vec3(0.2, 0.0, 0.0),
+    friction_coeff=0.001,
+    moment_of_inertia=(Vec3(0.0404, 0.0148202733120976, 0.0148202733120976))
+)
 
-    # create our first rocket
-    rocket_1: rocket = rocket(
-        name="rocket_1",
-        log_values=True,
-        do_aero=True,
-        dry_mass=0.4,
-        refence_area=np.pi*(0.076/2)**2,
-        drag_coeff_forewards=1.35,
-        drag_coeff_sideways=7.0,
-        cp_location=Vec3(0.2, 0.0, 0.0),
-        friction_coeff=0.001,
-        moment_of_inertia=(Vec3(0.0404, 0.0148202733120976, 0.0148202733120976))
-    )
+def get_time() -> float:
+    return np.random.normal(0, 1, 1)[0]
 
-    # make a tvc mount for said rocket
-    tvc_1: TVC = TVC(
-        name="tvc_1",
-        log_data=True,
-        speed=13.0,
-        maxPosition=Vec3(0.0, 0.1, 0.1),
-        minPosition=Vec3(0.0, -0.1, -0.1),
-        noise=0.001
-    )
+rocket_1.add_data_point("T", get_time)
 
-    PID_y: torque_PID = torque_PID(Kp=25, Ki=0.0, Kd=10, inertia=rocket_1.body.moment_of_inertia.y, lever_arm=0.2)
-    PID_z: torque_PID = torque_PID(Kp=25, Ki=0.0, Kd=10, inertia=rocket_1.body.moment_of_inertia.z, lever_arm=0.2)
+# make a tvc mount for said rocket
+tvc_1: TVC = TVC(
+    name="tvc_1",
+    log_data=True,
+    speed=13.0,
+    maxPosition=Vec3(0.0, 0.1, 0.1),
+    minPosition=Vec3(0.0, -0.1, -0.1),
+    noise=0.0005
+)
 
-    @tvc_1.update_func
-    def tvc1_update():
+PID_y: PID = PID(Kp=0.2, Ki=0.1, Kd=0.1)
+PID_z: PID = PID(Kp=0.2, Ki=0.1, Kd=0.1)
 
-        target_vector: Vec3 = Vec3(1.0, 0.0, 0.0)
+@tvc_1.update_func
+def tvc1_update():
 
-        if simulation.time > 0.5:
-            target_vector.y += 0.1
+    target_vector: Vec3 = Vec3(1.0, 0.0, 0.0)
 
-        target_vector = rocket_1.body.rotation.conjugate().rotate(target_vector.normalize())
+    if simulation.time > 0.5:
+        target_vector.y += 0.1
 
-        rotVel = rocket_1.body.rotation.conjugate().rotate(rocket_1.body.rotational_velocity)
+    target_vector = rocket_1.body.rotation.conjugate().rotate(target_vector.normalize())
 
-        PID_y.update(np.arctan2(-target_vector.z, target_vector.x) * PID_z.getOutput()*(180/np.pi), tvc_1.update_delay, rocket_1.motor_thrust, rotVel.y)
-        PID_z.update(np.arctan2(target_vector.y, target_vector.x) * PID_z.getOutput()*(180/np.pi), tvc_1.update_delay, rocket_1.motor_thrust, rotVel.z)
+    rotVel = rocket_1.body.rotation.conjugate().rotate(rocket_1.body.rotational_velocity)
 
-        return Vec3(0.0, PID_y.getOutput()*(180/np.pi), PID_z.getOutput()*(180/np.pi))
+    PID_y.update(np.arctan2(-target_vector.z, target_vector.x), tvc_1.update_delay, rotVel.y)
+    PID_z.update(np.arctan2(target_vector.y, target_vector.x), tvc_1.update_delay, rotVel.z)
+    # print(PID_z.getOutput()*(180/np.pi))
+    # print( rotVel.z*(180/np.pi))
+    return Vec3(0.0, PID_y.getOutput(), PID_z.getOutput())
 
-    # motor for the mount
-    motor_1: rocket_motor = rocket_motor(
-        filePath="C:/Users/Cameron/Documents/vscode/rockets/simulation/G-FIELD/motors/Estes_C6.rse", timeStep=1000)
+# motor for the mount
+motor_1: rocket_motor = rocket_motor(
+    filePath="C:/Users/Cameron/Documents/vscode/rockets/simulation/G-FIELD/motors/Estes_C6.rse", timeStep=1000)
 
-    parachute_1: parachute = parachute(diameter=0.9)
+parachute_1: parachute = parachute(diameter=0.9)
 
-    @parachute_1.deploy_func
-    def parachute_1_deploy_func(position: Vec3, velocity: Vec3):
-        
-        if position.x < 20.0 and velocity.x < -1.0:
-            return True
-        else:
-            return False
+@parachute_1.deploy_func
+def parachute_1_deploy_func(position: Vec3, velocity: Vec3):
+    
+    if position.x < 20.0 and velocity.x < -1.0:
+        return True
+    else:
+        return False
 
-    # add the motor to the mount
-    tvc_1.add_motor(motor_1)
+# add the motor to the mount
+tvc_1.add_motor(motor_1)
 
-    # add the mount to the rocket
-    rocket_1.add_tvc_mount(tvc_mount=tvc_1, position=Vec3(-0.2, 0.0, 0.0))
+# add the mount to the rocket
+rocket_1.add_tvc_mount(tvc_mount=tvc_1, position=Vec3(-0.2, 0.0, 0.0))
 
-    # add the parachute to the rocket
-    rocket_1.add_parachute(parachute_1, Vec3(0.0, 0.0, 0.0))
+# add the parachute to the rocket
+rocket_1.add_parachute(parachute_1, Vec3(0.0, 0.0, 0.0))
 
-    # add the rocket to the sim
-    simulation.add_rocket(rocket_1)
+# add the rocket to the sim
+simulation.add_rocket(rocket_1)
 
-    # set motor ignition time
-    motor_1.set_ignition_time(0.0)
-
-    simulation.run()
+# set motor ignition time
+motor_1.set_ignition_time(0.0)
 
 if __name__ == "__main__":
-    main()
\ No newline at end of file
+    simulation.run()
\ No newline at end of file