Merge pull request #32 from akruszew/voltage_control

Comm. Timing + PWM mode + examples

Author: HanaeRateau

emioapi/_emiomotorsparameters.py

@@ -34,12 +34,15 @@
 VELOCITY_MODE               = 1
 POSITION_MODE               = 3
 EXT_POSITION_MODE           = 4
+PWM_MODE                    = 16
 if MY_DXL == 'X_SERIES' or MY_DXL == 'MX_SERIES': #from https://emanual.robotis.com/docs/en/dxl/x/xm430-w210/
     ADDR_TORQUE_ENABLE          = 64
     ADDR_GOAL_POSITION          = 116
     LEN_GOAL_POSITION           = 4         # Data Byte Length
     ADDR_GOAL_VELOCITY          = 104
     LEN_GOAL_VELOCITY           = 4         # Data Byte Length
+    ADDR_GOAL_PWM               = 100
+    LEN_GOAL_PWM                = 2
     ADDR_PRESENT_POSITION       = 132
     LEN_PRESENT_POSITION        = 4         # Data Byte Length
     ADDR_PRESENT_VELOCITY       = 128

emioapi/_motorgroup.py

@@ -145,6 +145,9 @@ def __init__(self, parameters: MotorsParametersTemplate) -> None:
         self.groupWriters["goal_velocity"] = GroupSyncWrite(self.portHandler, self.packetHandler,
                                                             self.parameters.ADDR_GOAL_VELOCITY,
                                                             self.parameters.LEN_GOAL_POSITION)
+        self.groupWriters["goal_pwm"] = GroupSyncWrite(self.portHandler, self.packetHandler,
+                                                            self.parameters.ADDR_GOAL_PWM,
+                                                            self.parameters.LEN_GOAL_PWM)
         self.groupWriters["velocity_profile"] = GroupSyncWrite(self.portHandler, self.packetHandler,
                                                                 self.parameters.ADDR_VELOCITY_PROFILE,
                                                                 self.parameters.LEN_GOAL_POSITION)
@@ -222,8 +225,6 @@ def _readSyncMotorsData(self, groupSyncRead:GroupSyncRead):
         Raises:
             Exception: If the motor group is not connected or if the read operation fails.
         """
-        if not self.isConnected:
-            raise DisconnectedException()
 
         dxl_comm_result = groupSyncRead.txRxPacket()
         if dxl_comm_result != COMM_SUCCESS:
@@ -246,9 +247,7 @@ def __writeSyncMotorsData(self, group: GroupSyncWrite, values):
             group (GroupSyncWrite): The group sync write object.
             values (list of numbers): The values to write to the motors.
         """
-        if not self.isConnected:
-            raise DisconnectedException()
-
+        
         group.clearParam()
         for index, DXL_ID in enumerate(self.parameters.DXL_IDs):
             if group.data_length == 2:
@@ -314,6 +313,15 @@ def enableExtendedPositionMode(self):
         if any(t==1 for t in torques):
             self.enableTorque()
 
+    def enablePWMMode(self):
+        torques = self.isTorqueEnable()
+
+        if any(t==1 for t in torques):
+            self.disableTorque()
+        self.__setOperatingMode(self.parameters.PWM_MODE)
+        if any(t==1 for t in torques):
+            self.enableTorque()
+
 
     def enablePositionMode(self):
         torques = self.isTorqueEnable()
@@ -342,6 +350,13 @@ def setGoalPosition(self, positions):
         """
         self.__writeSyncMotorsData(self.groupWriters["goal_position"], positions)
 
+    def setGoalPWM(self, pwms):
+        """Set the goal velocity
+
+        Args:
+            speeds (list of numbers): unit depends on motor type
+        """
+        self.__writeSyncMotorsData(self.groupWriters["goal_pwm"] , pwms)
 
     def setVelocityProfile(self, max_vel):
         """Set the maximum velocities in position mode

emioapi/emiomotors.py

@@ -46,6 +46,7 @@ class EmioMotors:
     _max_vel: float = 1000  # *0.01 rev/min
     _goal_velocity: list = field(default_factory=lambda: [0] * len(emioparameters.DXL_IDs))
     _goal_position: list = field(default_factory=lambda: [0] * len(emioparameters.DXL_IDs))
+    _goal_pwm: list = field(default_factory=lambda: [0] * len(emioparameters.DXL_IDs))
     _mg: motorgroup.MotorGroup = None
     _device_index: int = None
 
@@ -222,6 +223,8 @@ def enablePositionMode(self):
     def enableExtendedPositionMode(self):
         self._mg.enableExtendedPositionMode()
 
+    def enablePWMMode(self):
+        self._mg.enablePWMMode()
 
     ####################
     #### PROPERTIES ####
@@ -275,6 +278,18 @@ def goal_velocity(self, velocities: list):
             self._mg.setGoalVelocity(velocities)
 
 
+    @property
+    def goal_pwm(self) -> list:
+        """Get the current velocity (rev/min) of the motors."""
+        return self._goal_pwm
+
+    @goal_pwm.setter
+    def goal_pwm(self, pwms: list):
+        """Set the goal velocity (rev/min) of the motors."""
+        self._goal_pwm = pwms
+        with self._lock:
+            self._mg.setGoalPWM(pwms)
+
     @property
     def max_velocity(self)-> list:
         """Get the current velocity (rev/min) profile of the motors."""

examples/motors_pid_position.py

@@ -1,7 +1,11 @@
 import numpy as np
+import matplotlib
+matplotlib.use("TkAgg")
 import matplotlib.pyplot as plt
 import time
-
+import sys
+import os
+sys.path.append(os.path.dirname(os.path.realpath(__file__))+'/..')
 from emioapi import EmioMotors
 
 
@@ -15,8 +19,8 @@ def main():
     print("Motors opened successfully.")
 
     # initial and target angles
-    init_angles = np.array([0.5, 0, 0.5, 0])
-    target_angles = init_angles + np.array([0.4, 0, 0, 0])
+    init_angles = np.array([0.0, 0, 0.0, 0])
+    target_angles = init_angles + np.array([0.1, 0, 0, 0])
     motors.angles = init_angles
     time.sleep(1)
 
@@ -42,7 +46,6 @@ def main():
     while time.time() - t0 < 0.3:
         measures.append(motors.angles)
         times.append(time.time())
-        time.sleep(0.01)
     time.sleep(1)
     nb_steps1 = len(measures)
 
@@ -51,7 +54,7 @@ def main():
 
     motors.position_p_gain = [15800, 800, 15800, 800]
     motors.position_i_gain = [0, 0, 0, 0]
-    motors.position_d_gain = [600, 0, 600, 0]
+    motors.position_d_gain = [0, 0, 0, 0]
     print("Set second PID gains.")
     time.sleep(1)
 
@@ -70,7 +73,6 @@ def main():
     while time.time() - t0 < 0.3:
         measures.append(motors.angles)
         times.append(time.time())
-        time.sleep(0.01)
     time.sleep(1)
 
     motors.close()

examples/motors_pwm.py

@@ -0,0 +1,102 @@
+#!/usr/bin/env -S uv run --script
+
+import time
+import logging
+import os
+import sys
+import matplotlib
+matplotlib.use("TkAgg")
+import matplotlib.pyplot as plt
+import numpy as np
+sys.path.append(os.path.dirname(os.path.realpath(__file__))+'/..')
+from emioapi import EmioMotors
+from emioapi._logging_config import logger
+
+'''
+This example demonstrates how to use the EMIO API to control DYNAMIXEL motors in PWM mode.
+'''
+
+def main(emio: EmioMotors, loops=500, motor_id=1):
+    '''
+        Main function to run the PWM test.
+        Parameters:
+        - emio: An instance of the EmioMotors class.
+        - loops: Number of iterations to run the test.
+        - motor_id: ID of the motor to test (0-3).
+    '''
+    # Enable PWM mode
+    emio.enablePWMMode()
+    time.sleep(1)
+    emio.printStatus()
+
+    # Intialize lists to store data for plotting
+    start = time.perf_counter()
+    last = start
+    dt = []
+    speed = []
+    pwm = []
+    t = []
+    pwm_value = [0]*4
+
+    # Loop to set PWM values and read velocity
+    for i in range(loops):
+        # Record loop time and elapsed time
+        new = time.perf_counter()
+        dt.append(new-last)
+        t.append(new-start)
+        last = new
+
+        # Alternate PWM values for the first half and second half of the loops
+        if i<loops/2:
+            pwm_value[motor_id] = 200
+        else:
+            pwm_value[motor_id] = 400
+
+        emio.goal_pwm = pwm_value
+        pwm.append(pwm_value[motor_id])
+        
+        velocity = emio.velocity
+        speed.append(velocity[motor_id])
+                
+            
+    emio.goal_pwm=[0]*4
+    logger.info("PWM test completed. Plotting results...")
+    logger.info(f"Average loop time: {np.mean(dt):.4f} seconds")
+    logger.info("Plotting PWM and velocity over time...")
+    
+    fig, (ax1, ax2) = plt.subplots(2, 1, sharex=True)
+    ax1.plot(t, pwm, '-+')
+    ax1.legend(["pwm"])
+    ax2.plot(t, speed, '-+')
+    ax2.legend(["velocity"])
+    plt.show()
+
+if __name__ == "__main__":
+    emio_motors = None
+    try:
+        logger.info("Starting EMIO API test...")
+        logger.info("Opening and configuring EMIO API...")      
+        emio_motors = EmioMotors()
+        
+        if emio_motors.open(): 
+            
+            emio_motors.printStatus()
+
+            logger.info("Emio motors opened and configured.")
+            logger.info("Running main function...")
+            main(emio_motors, 500,1)
+
+            logger.info("Main function completed.")
+            logger.info("Closing Emio motor connection...")
+
+            emio_motors.close()
+            logger.info("Emio connection closed.")
+    except KeyboardInterrupt:
+        if emio_motors:
+            emio_motors.goal_pwm=[0]*4
+            plt.close("all")
+            emio_motors.close()
+    except Exception as e:
+        logger.exception(f"An error occurred: {e}")
+        if emio_motors:
+            emio_motors.close()

pyproject.toml

@@ -72,3 +72,8 @@ Class = 2
 Method = 3
 Function = 3
 Data = 3
+
+[dependency-groups]
+dev = [
+    "matplotlib>=3.10.8",
+]