Thread and mailbox incorporation

.gitignore

@@ -2,4 +2,5 @@
 *.pyc
 *.log
 py39-venv/
-py311-venv/
+py311-venv/
+*.mat

dephyEB51.py

@@ -27,6 +27,7 @@
     TEST_TR_FILE
 )
 from src.exo.variable_transmission_ratio import VariableTransmissionRatio
+from gse_imu import IMU_Estimator
 
 
 class DephyEB51Actuator(DephyLegacyActuator):
@@ -63,6 +64,7 @@ def __init__(
         dephy_log: bool = False,
         offline: bool = False,
         gear_ratio: float = 1.0,
+        imu_estimator: object = None
     ) -> None:
 
         CONSOLE_LOGGER.info("Initializing DephyEB51 actuator...")
@@ -109,9 +111,11 @@ def __init__(
         self.case_temp_buffer = []
 
         # instantiate transmission ratio getter which uses motor-angle curve coefficients from pre-performed calibration
-        self.tr_gen = VariableTransmissionRatio(self.side, TEST_TR_FILE)
+        self.tr_gen = VariableTransmissionRatio(self.side)
         CONSOLE_LOGGER.info("instantiated variable transmission ratio")
 
+        # instantiate IMU-based gait-state estimator
+        self.imu_estimator = IMU_Estimator()
 
     def update_gear_ratio(self)-> None:
         """
@@ -131,9 +135,10 @@ def ankle_angle(self) -> float:
         Ankle angle is in ° and is the angle of the ankle joint using the ankle encoder. Angles should range anywhere from 0° to 140°.
         """
 
+        # TODO look over ank_ang/100 versus ank_ang*ENC_CLICKS_TO_DEG
         if self._data is not None:
-            ank_ang_in_deg = self.ank_ang/100
-            return float( (self.ank_enc_sign * ank_ang_in_deg) - self.tr_gen.get_offset() )
+            ank_ang_in_deg = self.ank_ang * EB51_CONSTANTS.MOT_ENC_CLICKS_TO_DEG
+            return float((self.ank_enc_sign * ank_ang_in_deg) - self.tr_gen.get_offset())
         else:
             LOGGER.debug(
                 msg="Actuator data is none, please ensure that the actuator is connected and streaming. Returning 0.0."
@@ -145,15 +150,29 @@ def update(self):
         Updates the actuator state.
         """
 
-        # filter the temperature before updating the thermal model
-        self.filter_temp()
-
         # update the actuator state
+        # super().update_allData()
         super().update()
 
+        # filter the temperature before updating the thermal model
+        self.filter_temp()
+
         # update the gear ratio
         self.update_gear_ratio()
 
+        # update gait state estimate
+        self.imu_estimator.update(self.accelz, self.ankle_angle)
+
+    def get_imu_gait_state_estimate(self, var:str):
+        """
+        Returns the IMU's gait state estimate
+
+        Args:
+            - desired variable from imu estimate to return
+        """
+        state_dict = self.imu_estimator.return_estimate()
+
+        return state_dict[var]
 
     def assign_id_to_side(self)-> str:
         """
@@ -167,15 +186,11 @@ def spool_belt(self):
 
         LOGGER.info(
             f"Spooling {self.side} joint. "
-            "Please make sure the joint is free to move and press Enter to continue."
+            "Please make sure the joint is free to move."
         )
 
-        input()
         self.set_motor_current(value=self.motor_sign * EXO_DEFAULT_CONFIG.BIAS_CURRENT)  # in mA
 
-        time.sleep(0.3)
-
-
     def filter_temp(self):
         """
         Filters the case temperature to remove any spikes.
@@ -197,7 +212,6 @@ def filter_temp(self):
 
                 CONSOLE_LOGGER.warning(f"HAD TO ANTI-SPIKE the TEMP: {self.case_temperature}")
 
-
     def torque_to_current(self, torque: float) -> int:
         """
         Converts torque setpoint (Nm) to a corresponding current (in mA)
@@ -220,8 +234,6 @@ def torque_to_current(self, torque: float) -> int:
 
         return int(des_current)
 
-
-
     def current_to_torque(self)-> float:
         """
         Converts current setpoint (in mA) to a corresponding torque (in Nm)
@@ -248,4 +260,4 @@ def JIM_torque_to_current(self, inst_torque: float) -> int:
         """
         pass
 
-    # TODO: Add method to home the exos at standing angle
+    # TODO: Add method to home the exos at standing angle