Merge pull request #276 from neurobionics/integration

Final dephy debugging

opensourceleg/actuators/dephy.py

@@ -184,6 +184,7 @@ def _CONTROL_MODE_CONFIGS(self) -> CONTROL_MODE_CONFIGS:
     def start(self) -> None:
         try:
             self.open()
+            self._is_open = True
         except OSError as e:
             print("\n")
             LOGGER.error(
@@ -192,19 +193,20 @@ def start(self) -> None:
             os._exit(status=1)
 
         self.start_streaming(self._frequency)
-        self._data = self.read()
-
-        time.sleep(0.1)
+        time.sleep(0.2)
+        self._is_streaming = True
 
+        self._data = self.read()
         self.set_control_mode(CONTROL_MODES.VOLTAGE)
 
     @check_actuator_stream
     @check_actuator_open
     def stop(self) -> None:
-        self.set_control_mode(mode=CONTROL_MODES.VOLTAGE)
-        self.stop_motor()
 
-        time.sleep(0.1)
+        self.stop_motor()
+        self.set_control_mode(mode=CONTROL_MODES.IDLE)
+        self._is_streaming = False
+        self._is_open = False
         self.close()
 
     def update(self) -> None:
@@ -220,6 +222,7 @@ def update(self) -> None:
             LOGGER.error(
                 msg=f"[{str.upper(self.tag)}] Case thermal limit {self.max_case_temperature} reached. Stopping motor."
             )
+            # self.stop()
             raise ThermalLimitException()
 
         if self.winding_temperature >= self.max_winding_temperature:
@@ -229,6 +232,7 @@ def update(self) -> None:
             raise ThermalLimitException()
         # Check for thermal fault, bit 2 of the execute status byte
         if self._data["status_ex"] & 0b00000010 == 0b00000010:
+            self.stop()
             raise RuntimeError("Actpack Thermal Limit Tripped")
 
     def home(
@@ -1065,11 +1069,9 @@ def stop(self) -> None:
         self.set_control_mode(mode=CONTROL_MODES.VOLTAGE)
         self.set_motor_voltage(value=0)
 
-        # time.sleep(0.2)
         self.set_control_mode(mode=CONTROL_MODES.IDLE)
         time.sleep(0.1)
         self.close()
-        # time.sleep(0.1)
 
     def update(self) -> None:
 

tutorials/actuators/dephy/current_control.py

@@ -2,6 +2,7 @@
 
 import pandas as pd
 
+from opensourceleg.actuators.base import CONTROL_MODES
 import opensourceleg.actuators.dephy as Dephy
 from opensourceleg.logging.logger import LOGGER
 from opensourceleg.time import SoftRealtimeLoop
@@ -27,7 +28,7 @@ def main():
     with actpack:
 
         try:
-            actpack.set_control_mode(mode=actpack.CONTROL_MODES.CURRENT)
+            actpack.set_control_mode(mode=CONTROL_MODES.CURRENT)
             actpack.set_current_gains(
                 # if no input, then default gains are applied
             )

tutorials/actuators/dephy/sensor_reading.py

@@ -1,4 +1,5 @@
 import time
+import sys
 
 import opensourceleg.actuators.dephy as Dephy
 from opensourceleg.logging.logger import LOGGER
@@ -9,16 +10,21 @@
 )
 with actpack:
     try:
+        print("Case:", actpack.case_temperature)
+        print("Winding:", actpack.winding_temperature)
         while True:
+            print(actpack._data)
             actpack.update()
             LOGGER.info(
                 "".join(
                     f"Motor Position: {actpack.motor_position}\t"
                     + f"Motor Voltage: {actpack.motor_voltage}\t"
                     + f"Motor Current: {actpack.motor_current}\t"
+                    + f"Case Temperature: {actpack.case_temperature}"
+                    + f"Winding Temperature: {actpack.winding_temperature}"
                 )
             )
-            time.sleep(0.1)
+            time.sleep(0.005)
 
-    except KeyboardInterrupt:
+    except Exception:
         exit()

tutorials/actuators/dephy/voltage_control.py

@@ -1,26 +1,102 @@
-import time
+# import time
+# import sys
+# from opensourceleg.actuators.base import CONTROL_MODES
+# import opensourceleg.actuators.dephy as Dephy
+# from opensourceleg.logging.logger import LOGGER
 
+# actpack = Dephy.DephyActuator(
+#     port="/dev/ttyACM0",
+#     gear_ratio=1.0,
+# )
+
+# with actpack:
+
+#     actpack.set_control_mode(mode=CONTROL_MODES.VOLTAGE)
+
+#     while True:
+#         actpack.set_motor_voltage(value=3000)  # in mV
+#         actpack.update()
+#         LOGGER.info(
+#             "".join(
+#                 f"Motor Position: {actpack.motor_position}\t"
+#                 + f"Case Temperature: {actpack.case_temperature}"
+#                 + f"Winding Temperature: {actpack.winding_temperature}"
+#             )
+#         )
+#         time.sleep(0.005)
+
+
+
+import time
+
+import pandas as pd
+
+from opensourceleg.actuators.base import CONTROL_MODES
 import opensourceleg.actuators.dephy as Dephy
 from opensourceleg.logging.logger import LOGGER
+from opensourceleg.time import SoftRealtimeLoop
+
+TIME_TO_STEP = 1.0
+FREQUENCY = 200
+DT = 1 / FREQUENCY
+
 
-actpack = Dephy.DephyActuator(
-    port="/dev/ttyACM0",
-    gear_ratio=1.0,
-)
-with actpack:
-    try:
-        actpack.set_control_mode(mode=actpack.CONTROL_MODES.VOLTAGE)
-        while True:
-            actpack.set_motor_voltage(value=3000)  # in mV
-            actpack.update()
-            LOGGER.info(
-                "".join(
-                    f"Motor Position: {actpack.motor_position}\t"
-                    + f"Motor Voltage: {actpack.motor_voltage}\t"
-                    + f"Motor Current: {actpack.motor_current}\t"
+def main():
+    actpack = Dephy.DephyActuator(
+        port="/dev/ttyACM0",
+        gear_ratio=1.0,
+    )
+    voltage_data = pd.DataFrame(
+        {
+            "Time": [],
+            "Output_Voltage": [],
+            "Command_Voltage": [],
+        }
+    )
+    clock = SoftRealtimeLoop(dt=DT)
+    with actpack:
+
+        try:
+            actpack.set_control_mode(mode=CONTROL_MODES.VOLTAGE)
+
+            for t in clock:
+
+                if t > TIME_TO_STEP:
+                    command_voltage = 3000
+                    actpack.set_motor_voltage(value=command_voltage)  # in mV
+
+                else:
+                    command_voltage = 0
+
+                actpack.update()
+
+                LOGGER.info(
+                    "".join(
+                        f"Motor Position: {actpack.motor_position}\t"
+                        + f"Motor Voltage: {actpack.motor_voltage}\t"
+                        + f"Motor Current: {actpack.motor_current}\t"
+                    )
+                )
+                voltage_data = pd.concat(
+                    [
+                        voltage_data,
+                        pd.DataFrame(
+                            {
+                                "Time": [t],
+                                "Output_Voltage": [actpack.motor_voltage],
+                                "Command_Voltage": [command_voltage],
+                            }
+                        ),
+                    ],
+                    ignore_index=True,
                 )
-            )
-            time.sleep(0.005)
 
-    except KeyboardInterrupt:
-        exit()
+                time.sleep(DT)
+
+        finally:
+            voltage_data.to_csv("voltage_data_dephy.csv", index=False)
+            exit()
+
+
+if __name__ == "__main__":
+    main()