Python 2.7 如何使伺服DynamicXel电机从0移动-->；90-->；180-->；220--->；零度

你好，软件开发人员。 我是编程和ROS方面的新手。我现在正在使用伺服电机，希望使控制节点将电机旋转到一些角度，如0-->90-->180-->0度。 有一个主代码，我想在主节点中添加def函数：

#!/usr/bin/env python

import os
import rospy
from std_msgs.msg import String
from controller_motor.msg import Motor
from controller_motor.srv import Motor2


if os.name == 'nt':
    import msvcrt
    def getch():
        return msvcrt.getch().decode()
else:
    import sys, tty, termios
    fd = sys.stdin.fileno()
    old_settings = termios.tcgetattr(fd)
    def getch():
        try:
            tty.setraw(sys.stdin.fileno())
            ch = sys.stdin.read(1)
        finally:
            termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
        return ch

from dynamixel_sdk import *                    # Uses Dynamixel SDK library


class DynamixelController(object):
    def __init__(self):
        #print("1")

        self.motor_sub = rospy.Subscriber("/motor_topic", Motor, self.motor_callback)
        self.test_motor_client(1)                #here we can put request order as 1 for the using id=14 and position= 1500; or as 2 is id same and position is 2500. The cases could be found in the server.py node
    
    def motor_callback(self, data):
        _id = data.id
        _position = data.position

        print("_id : {0}, _position : {1}".format(_id, _position))
        self.read_write(_id, _position)

    def test_motor_client(self, request):
        print("2")
        rospy.wait_for_service('test_motor_service')
        try:
            proxy = rospy.ServiceProxy('test_motor_service', Motor2)
            response = proxy(request)
            print("response is id : {} position : {}".format(response.id, response.position))

            self.read_write(response.id, response.position)

        except rospy.ServiceException as e:
            print("Service call failed: %s"%e)

    def new_rotation(self, response):
        print("3")
        rospy.wait_for_service('test_mototr_rotation')
        try:
            proxy = rospy.ServiceProxy('test_motor_rotation', Motor2)
            response = proxy(request)
            self.read_write(response.id, response.position(arg[0]))
            sleep(2)
            self.read_write(response.id, response._position(arg[1]))
            sleep(2)
            self.read_write(response.id, response._position(arg[2]))
            sleep(2)
        except rospy.ServiceException as e:
            print("Service call failed: %s"%e)
    

    
    def read_write(self, _id, _position):
        print("Start the node")
     

        # Control table address
        ADDR_PRO_TORQUE_ENABLE      = 64                  # Control table address is different in Dynamixel model
        ADDR_PRO_GOAL_POSITION      = 116
        ADDR_PRO_PRESENT_POSITION   = 132
       
        # Protocol version
        PROTOCOL_VERSION            = 2.0                 # See which protocol version is used in the Dynamixel

        # Default setting
        DXL_ID                      = _id                 # Dynamixel ID : 14
        BAUDRATE                    = 1000000             # Dynamixel default baudrate : 1000000
        DEVICENAME                  = '/dev/ttyUSB0'      # Check which port is being used on your controller
                                                          # ex) Windows: "COM1"   Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*"

        TORQUE_ENABLE               = 1                 # Value for enabling the torque
        TORQUE_DISABLE              = 0                 # Value for disabling the torque
        DXL_MINIMUM_POSITION_VALUE  = 10                # Dynamixel will rotate between this value
        DXL_MAXIMUM_POSITION_VALUE  = _position         # The position here is already in the degree measurement; and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.)
        DXL_MOVING_STATUS_THRESHOLD = 20                # Dynamixel moving status threshold
        

        index = 0
        dxl_goal_position = [DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE]         # Goal position
        


        # Initialize PortHandler instance
        # Set the port path
        # Get methods and members of PortHandlerLinux or PortHandlerWindows
        portHandler = PortHandler(DEVICENAME)

        # Initialize PacketHandler instance
        # Set the protocol version
        # Get methods and members of Protocol1PacketHandler or Protocol2PacketHandler
        packetHandler = PacketHandler(PROTOCOL_VERSION)

        # Open port
        if portHandler.openPort():
            print("Succeeded to open the port")
        else:
            print("Failed to open the port")
            print("Press any key to terminate...")
            getch()
            quit()


        # Set port baudrate
        if portHandler.setBaudRate(BAUDRATE):
            print("Succeeded to change the baudrate")
        else:
            print("Failed to change the baudrate")
            print("Press any key to terminate...")
            getch()
            quit()

        # Enable Dynamixel Torque
        dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE)
        if dxl_comm_result != COMM_SUCCESS:
            print("%s" % packetHandler.getTxRxResult(dxl_comm_result))
        elif dxl_error != 0:
            print("%s" % packetHandler.getRxPacketError(dxl_error))
        else:
            print("Dynamixel has been successfully connected")

        while 1:
            print("Press any key to continue! (or press ESC to quit!)")
            if getch() == chr(0x1b):
                break

            # Write goal position
            dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, DXL_ID, ADDR_PRO_GOAL_POSITION, dxl_goal_position[index])
            if dxl_comm_result != COMM_SUCCESS:
                print("%s" % packetHandler.getTxRxResult(dxl_comm_result))
            elif dxl_error != 0:
                print("%s" % packetHandler.getRxPacketError(dxl_error))

            while 1:
                # Read present position
                dxl_present_position, dxl_comm_result, dxl_error = packetHandler.read4ByteTxRx(portHandler, DXL_ID, ADDR_PRO_PRESENT_POSITION)
                if dxl_comm_result != COMM_SUCCESS:
                    print("%s" % packetHandler.getTxRxResult(dxl_comm_result))
                elif dxl_error != 0:
                    print("%s" % packetHandler.getRxPacketError(dxl_error))

                print("[ID:%03d] GoalPos:%03d  PresPos:%03d" % (DXL_ID, dxl_goal_position[index], dxl_present_position))

                if not abs(dxl_goal_position[index] - dxl_present_position) > DXL_MOVING_STATUS_THRESHOLD:
                    break

            # Change goal position
            if index == 0:
                index = 1
            else:
                index = 0


        # Disable Dynamixel Torque
        dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE)
        if dxl_comm_result != COMM_SUCCESS:
            print("%s" % packetHandler.getTxRxResult(dxl_comm_result))
        elif dxl_error != 0:
            print("%s" % packetHandler.getRxPacketError(dxl_error))

        # Close port
        portHandler.closePort()
if __name__ == '__main__':
    rospy.init_node('read_write', anonymous=True)
    controller = DynamixelController()

这是服务器节点：

#!/usr/bin/env python
import rospy
from controller_motor.srv import Motor2, Motor2Response


def motor_info(req):
    print("receive the request : {}".format(req.order))
    if req.order == 1:
        _id = 14
        #_position = 1500
        _positiondegree = 180                 # change it degree value
        _position = (11.4 * (_positiondegree))  # here is using there 11.4 the number which helps rotate the motor in the correct degree; We get the number encoder number/degree we want to rotate the motor.
              
    elif req.order == 2:
        _id = 14
        #_position = 2047.5
        _positiondegree = 180
        _position = (11.4 * (_positiondegree))
       

    print("Response to this request id : {} and postiion : {}".format(_id, _position))
    return Motor2Response(_id, _position)
   

    
def motor_info_server():

    rospy.init_node('test_motor_service')
    s = rospy.Service('test_motor_service', Motor2, motor_info)
    print("Ready to response some id and position")

    # spin() simply keeps python from exiting until this node is stopped
    rospy.spin()

if __name__ == '__main__':
    motor_info_server()

现在，当运行电机时，它旋转到我希望它旋转的程度。现在我想知道如何给电机一些角度，使电机从0旋转到其他角度，然后再回到0度的位置

我猜在某些程度上可能会应用列表和args。但我不知道我是如何做到这一点的：） 我想从像你们这样的专业人士那里得到一些建议：）

我知道我的问题对专业人士来说是显而易见的。我希望尽可能多地获得编程和ROS方面的经验：）

我将非常感谢任何帮助，并建议如何做到这一点：）

感谢所有愿意分享他/她的想法的人：）

---

祝您度过愉快的一天：）

您必须在客户端文件的代码中添加一个新函数：

def rotate_bot(self,degree):
     rospy.wait_for_service('rotate_motor')
     try:
         proxy = rospy.ServiceProxy('test_motor_rotation', Motor2)
         response = proxy(degree)
     except rospy.ServiceException as e:
         print("Service call failed: %s"%e)

这个函数基本上调用一个服务，并给他机器人必须转动的度数。还添加此函数以调用roate函数：

def test_rotate(self):
       rotate_bot(90)
       rotate_bot(90)
       rotate_bot(-180)

现在，机器人从0-->90-->180-->0旋转 在服务器文件中，您必须在

motor\u info\u server（）

方法中添加服务：

s = rospy.Service('test_motor_rotation', Motor2, rotate_motor)

并添加了以下方法：

def rotate_motor(req):
    print("received the request : {}".format(req.order))
    degree = req.order
    _id = 14
    _positiondegree = degree                 
    _position = (11.4 * (_positiondegree))  

    return Motor2Response(_id, _position)

---

如果你想通过双倍学位，你必须将你的信息数据类型从Motor2改为其他的…

我对ROS不太了解，但作为一个一般提示，你发布了太多的代码。我估计您显示的代码中至少有90%与问题（电机旋转）无关。如果您将代码示例减少到最小值（），则获得答案的几率会提高。通常这意味着从头开始重写它，所以它除了你有疑问的事情之外什么也不做。