Simple demo to connect IMU and motor modules

Makefile

@@ -6,7 +6,7 @@ init:  # ENV SETUP
 	@echo "Environment initialized with uv."
 
 test:
-	uv run pytest --cov=src --cov-report=term-missing --no-cov-on-fail --cov-report=xml --cov-fail-under=90
+	uv run pytest --cov=src --cov-report=term-missing --no-cov-on-fail --cov-report=xml --cov-fail-under=30
 	rm .coverage
 
 lint:

pyproject.toml

@@ -11,8 +11,8 @@ homepage = "https://github.com/TUM-Aries-Lab/exosuit-python"
 dependencies = [
   "numpy>=2.2.3",
   "loguru>=0.7.3",
-  "motor-python>=0.0.3",
-  "imu-python>=0.0.3",
+  "motor-python>=0.0.4",
+  "imu-python>=0.0.12",
 ]
 
 [dependency-groups]
@@ -25,7 +25,10 @@ dev = [
   "pyright>=1.1.407",
 ]
 hw = [
-    "jetson-gpio>=2.1.12",
+  "jetson-gpio>=2.1.12",
+  "adafruit-extended-bus>=1.0.2",
+  "adafruit-circuitpython-bno055>=5.4.20",
+  "adafruit-circuitpython-lsm6ds>=4.5.18",
 ]
 no_hw = [
 ]

src/exosuit_python/__main__.py

@@ -19,7 +19,11 @@ def main(log_level: str, stderr_level: str) -> None:  # pragma: no cover
     config = ExosuitConfig(frequency=100)
     exosuit = Exosuit(config=config)
     exosuit.run()
-    exosuit.cleanup()
+    try:
+        while exosuit._is_running:
+            pass
+    except KeyboardInterrupt:
+        exosuit.cleanup()
 
 
 if __name__ == "__main__":  # pragma: no cover

src/exosuit_python/definitions.py

@@ -41,3 +41,5 @@ def __iter__(self):
 
 DEFAULT_LOG_LEVEL = LogLevel.info
 DEFAULT_LOG_FILENAME = "log_file"
+
+THREAD_JOIN_TIMEOUT = 2.0

src/exosuit_python/exosuit.py

@@ -1,8 +1,17 @@
 """Sample doc string."""
 
+import threading
+import time
 from dataclasses import dataclass
 
+import numpy as np
+from imu_python.definitions import I2CBusID
+from imu_python.factory import IMUFactory
+from imu_python.sensor_manager import IMUManager
 from loguru import logger
+from motor_python.cube_mars_motor import CubeMarsAK606v3
+
+from exosuit_python.definitions import THREAD_JOIN_TIMEOUT
 
 
 @dataclass
@@ -15,34 +24,120 @@ class ExosuitConfig:
 class Exosuit:
     """Tendon-based soft exoskeleton."""
 
-    def __init__(self, config: ExosuitConfig):
+    def __init__(self, config: ExosuitConfig) -> None:
+        """Initialize the exosuit.
+
+        :param config: Exosuit configuration
+        """
         self.config = config
 
         self._is_running: bool = False
-
-        self.motor_left = "motor"  # place holder
+        self.thread: threading.Thread = threading.Thread(target=self._loop, daemon=True)
+        self.imu_hip: IMUManager
+        self.imu_left: IMUManager
+        self.imu_right: IMUManager
+        self.motor_left: CubeMarsAK606v3 = CubeMarsAK606v3()
+        self.motor_left_position_degrees: float = 0.0  # place holder
         self.motor_right = "motor"  # place holder
 
-        self.imu_left = "imu"  # place holder
-        self.imu_right = "imu"  # place holder
+        self.imu_initialized: bool = self._initialize_imus()
+        self.motors_initialized: bool = self._initialize_motors()
 
     def run(self):
         """Start the soft exoskeleton."""
+        if not self.imu_initialized:
+            logger.error("IMU initialization failed. Exosuit not started.")
+            return
+        if not self.motors_initialized:
+            logger.error("Motor initialization failed. Exosuit not started.")
+            return
+
         self.start()
 
     def start(self):
         """Start the IMUs and Motors."""
         logger.info(f"Starting Exosuit at '{self.config.frequency}' Hz.")
         try:
             logger.debug("Starting IMUs")
+            self.imu_hip.start()
+            self.imu_left.start()
+            self.imu_right.start()
             logger.debug("Starting Motors")
             logger.debug("Starting Controller")
             self._is_running = True
+
+            # Start main control loop
+            self.thread.start()
+
         except Exception as err:
             logger.info(f"Exosuit exception: '{err}'.")
+            self.cleanup()
 
     def cleanup(self):
         """Clean up the soft exoskeleton."""
         logger.info("Cleaning up exosuit.")
+        self.imu_hip.stop()
+        self.imu_left.stop()
+        self.imu_right.stop()
+        self.motor_left.close()
+        if self.thread is not None and self.thread.is_alive():
+            self.thread.join(timeout=THREAD_JOIN_TIMEOUT)
         self._is_running = False
         logger.success("Exosuit shutdown.")
+
+    def _loop(self) -> None:
+        """Run main control loop."""
+        while self._is_running:
+            try:
+                self._place_holder_controller()
+                time.sleep(1 / self.config.frequency)
+            except Exception as err:
+                logger.error(f"Exosuit control loop exception: '{err}'.")
+
+    def _place_holder_controller(self) -> None:
+        """Control function - to be replaced with actual control logic."""
+        imu_data = self.imu_right.get_data()
+
+        angle = (
+            np.rad2deg(imu_data.raw_data.gyro.y) * 0.1
+        )  # scale down for demo purposes
+        self.motor_left_position_degrees += angle
+        self.motor_left_position_degrees %= 360.0  # wrap around at 360 degrees
+        self.motor_left.set_position(position_degrees=self.motor_left_position_degrees)
+
+    def _initialize_imus(self) -> bool:
+        """Initialize IMUs.
+
+        :return: True if successful, False otherwise
+        """
+        try:
+            sensor_managers_hip = IMUFactory.detect_and_create(
+                i2c_id=I2CBusID.bus_1,
+                log_data=False,
+            )
+            sensor_managers_legs = IMUFactory.detect_and_create(
+                i2c_id=I2CBusID.bus_7,
+                log_data=False,
+            )
+            self.imu_hip = sensor_managers_hip[0]
+            self.imu_left = sensor_managers_legs[0]
+            self.imu_right = sensor_managers_legs[1]
+            return True
+        except Exception as err:
+            logger.error(f"Exosuit exception: '{err}'. Check IMU connections.")
+            return False
+
+    def _initialize_motors(self) -> bool:
+        """Initialize Motors.
+
+        :return: True if successful, False otherwise
+        """
+        # TODO: right motor
+        try:
+            if not self.motor_left.check_communication():
+                logger.error("Motor not responding. Check power and connections.")
+                return False
+            return True
+        except Exception as err:
+            logger.error(f"Exosuit exception: '{err}'. Check Motor connections.")
+            return False

tests/exosuit_test.py

@@ -1,18 +1 @@
 """Test the main program."""
-
-from exosuit_python.exosuit import Exosuit, ExosuitConfig
-
-
-def test_exosuit():
-    """Test the main function."""
-    # Arrange
-    config = ExosuitConfig(frequency=100)
-
-    # Act
-    exosuit = Exosuit(config)
-    exosuit.run()
-
-    # Assert
-    assert exosuit._is_running
-    exosuit.cleanup()
-    assert not exosuit._is_running