Add support for XL-320 Dynamixels

This code is very messy, made in a rush during RoboCup 2019
CCGSRobotics · Jul 8, 2019 · 2a5a4d1 · 2a5a4d1
1 parent 19749f1
commit 2a5a4d1
Show file tree

Hide file tree

Showing 9 changed files with 350 additions and 5 deletions.
diff --git a/Server/Server-Code-2019/GPIO.py b/Server/Server-Code-2019/GPIO.py
@@ -1,5 +1,5 @@
 import RPi.GPIO as GPIO
-from time import *
+import time
 
 pin = 20
 
@@ -12,6 +12,7 @@ def restart():
     GPIOsetup()
     GPIO.output(pin,1)
     print("[Dynamixels]: OFF")
+    time.sleep(3)
     GPIO.output(pin,0)
     print("[Dynamixels]: ON")
     return
diff --git a/Server/Server-Code-2019/NewRoboCupServer-Current.py b/Server/Server-Code-2019/NewRoboCupServer-Current.py
@@ -0,0 +1,77 @@
+import socketserver as SocketServer
+import sys
+from GPIO import *
+from emuBot import *
+from xl320 import *
+
+numberOfActiveDynamixels = 13
+
+def loopServoReset(setup):
+    for ID in range(1,numberOfActiveDynamixels+1):
+        try:
+            softwareResetServo(ID)
+            if ID == numberOfActiveDynamixels:
+                print("---------- // -----------")
+            if setup:
+                if (ID <= 4) or ID == 11:
+                    wheelMode(ID)
+                elif ID == 12 or ID == 13:
+                    cameraMode(ID)
+                else:
+                    jointMode(ID)
+        except:
+            print("Setup of ID: " + str(ID) + " failed.")
+
+loopServoReset(False)#True)
+cameraMode(12)
+cameraMode(13)
+lastValues = [0 for i in range(20)]
+
+
+class MyUCPHandler(SocketServer.BaseRequestHandler):
+    def handle(self):
+        self.data = self.request[0].decode('utf-8').strip()
+        new = self.data.split('\n')
+        for i in new:
+            if i != "":
+              if 0:
+                 print(new)
+              else:
+                if i == "restart":
+                    restart()
+                elif i == "softwareResetServos":
+                    loopServoReset(False)
+                else:
+                    ID = int(i[0]+i[1])
+                    if (ID <= 4) or (ID == 11):
+                        try:
+                            if lastValues[ID] != int(i[2:]):
+                                moveWheel(ID, int(i[2:]))
+                                lastValues[ID] = int(i[2:])
+                        except:
+                            print('brokeWheel ',ID)
+                    else:
+                        pos, speed = map(int, [round(float(x)) for x in i.split()[1:]])
+                        if ID == 12 or ID == 13:
+                            try:
+                                moveCamera(ID, pos)
+                            except:
+                                print("BrokeCamera", ID)
+                        else:
+                            try:
+                                moveJoint(ID, pos, speed)
+                            except:
+                                print(i[2:-4])
+                                print('BrokeJoint ', ID)
+               # print(i)
+           # else:
+            #    print(i)
+
+if __name__ == "__main__":
+    HOST, PORT = "", 9999
+
+SocketServer.UDPServer.allow_reuse_address = True
+server = SocketServer.UDPServer((HOST, PORT), MyUCPHandler)
+
+print('Dynamixel Server Started')
+server.serve_forever()
diff --git a/Server/Server-Code-2019/RoboCupServer-Current.py b/Server/Server-Code-2019/RoboCupServer-Current.py
@@ -21,7 +21,7 @@ def loopServoReset(setup):
 
 loopServoReset(True)
 
-lastValues = [0 for i in range(5)]
+lastValues = [0 for i in range(20)]
 
 
 class MyUCPHandler(SocketServer.BaseRequestHandler):
@@ -44,8 +44,7 @@ def handle(self):
                             if lastValues[ID] != int(i[2:]):
                                 moveWheel(ID, int(i[2:]))
                                 lastValues[ID] = int(i[2:])
-                        except e:
-                            print(e)
+                        except:
                             print('brokeWheel ',ID)
                     else:
                         pos, speed = map(int, [round(float(x)) for x in i.split()[1:]])

diff --git a/Server/Server-Code-2019/__pycache__/GPIO.cpython-35.pyc b/Server/Server-Code-2019/__pycache__/GPIO.cpython-35.pyc
diff --git a/Server/Server-Code-2019/__pycache__/emuBot.cpython-35.pyc b/Server/Server-Code-2019/__pycache__/emuBot.cpython-35.pyc
diff --git a/Server/Server-Code-2019/__pycache__/xl320.cpython-35.pyc b/Server/Server-Code-2019/__pycache__/xl320.cpython-35.pyc
diff --git a/Server/Server-Code-2019/camera.py b/Server/Server-Code-2019/camera.py
@@ -0,0 +1,129 @@
+import os, sys
+
+if os.name == 'nt':
+    import msvcrt
+    def getch():
+        return msvcrt.getch().decode()
+else:
+    import tty, termios
+    fd = sys.stdin.fileno()
+    old_settings = termios.tcgetattr(fd)
+    tty.setraw(sys.stdin.fileno())
+    def getch():
+        return sys.stdin.read(1)
+
+os.sys.path.append('../dynamixel_functions_py')             # Path setting
+
+import dynamixel_functions as dynamixel                     # Uses Dynamixel SDK library
+
+# Control table address
+ADDR_PRO_TORQUE_ENABLE       = 24                          # Control table address is different in Dynamixel model
+ADDR_PRO_GOAL_POSITION       = 30
+ADDR_PRO_PRESENT_POSITION    = 37
+
+# Protocol version
+PROTOCOL_VERSION            = 2                             # See which protocol version is used in the Dynamixel
+
+# Default setting
+DXL_ID                      = 13                             # Dynamixel ID: 1
+BAUDRATE                    = 1000000
+DEVICENAME                  = "/dev/ttyUSB0".encode('utf-8')        # Check which port is being used on your controller
+                                                            # ex) Windows: "COM1"   Linux: "/dev/ttyUSB0"
+
+TORQUE_ENABLE               = 1                             # Value for enabling the torque
+TORQUE_DISABLE              = 0                             # Value for disabling the torque
+DXL_MINIMUM_POSITION_VALUE  = 205                       # Dynamixel will rotate between this value
+DXL_MAXIMUM_POSITION_VALUE  = 819                        # 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
+
+ESC_ASCII_VALUE             = 0x1b
+
+COMM_SUCCESS                = 0                             # Communication Success result value
+COMM_TX_FAIL                = -1001                         # Communication Tx Failed
+
+# Initialize PortHandler Structs
+# Set the port path
+# Get methods and members of PortHandlerLinux or PortHandlerWindows
+port_num = dynamixel.portHandler(DEVICENAME)
+
+# Initialize PacketHandler Structs
+dynamixel.packetHandler()
+
+index = 0
+dxl_comm_result = COMM_TX_FAIL                              # Communication result
+dxl_goal_position = [DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE]         # Goal position
+
+dxl_error = 0                                               # Dynamixel error
+dxl_present_position = 0                                    # Present position
+
+# Open port
+if dynamixel.openPort(port_num):
+    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 dynamixel.setBaudRate(port_num, 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
+dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE)
+if dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) != COMM_SUCCESS:
+    dynamixel.printTxRxResult(PROTOCOL_VERSION, dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION))
+elif dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) != 0:
+    dynamixel.printRxPacketError(PROTOCOL_VERSION, dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION))
+else:
+    print("Dynamixel has been successfully connected")
+
+
+while 1:
+    print("Press any key to continue! (or press ESC to quit!)")
+    if getch() == chr(ESC_ASCII_VALUE):
+        break
+
+    # Write goal position
+    dynamixel.write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_GOAL_POSITION, dxl_goal_position[index])
+    if dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) != COMM_SUCCESS:
+        dynamixel.printTxRxResult(PROTOCOL_VERSION, dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION))
+    elif dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) != 0:
+        dynamixel.printRxPacketError(PROTOCOL_VERSION, dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION))
+
+    while 1:
+        # Read present position
+        dxl_present_position = dynamixel.read4ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_PRESENT_POSITION)
+        if dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) != COMM_SUCCESS:
+            dynamixel.printTxRxResult(PROTOCOL_VERSION, dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION))
+        elif dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) != 0:
+            dynamixel.printRxPacketError(PROTOCOL_VERSION, dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION))
+
+        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
+dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE)
+if dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) != COMM_SUCCESS:
+    dynamixel.printTxRxResult(PROTOCOL_VERSION, dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION))
+elif dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) != 0:
+    dynamixel.printRxPacketError(PROTOCOL_VERSION, dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION))
+
+
+# Close port
+dynamixel.closePort(port_num)
diff --git a/Server/Server-Code-2019/emuBot.py b/Server/Server-Code-2019/emuBot.py
@@ -5,7 +5,7 @@
 
 BAUDRATE = 1000000
 
-portHandler = PortHandler("/dev/ttyUSB0")
+portHandler = PortHandler("/dev/ttyUSB2")
 packetHandler = PacketHandler(1.0)
 
 if portHandler.openPort():

diff --git a/Server/Server-Code-2019/xl320.py b/Server/Server-Code-2019/xl320.py
@@ -0,0 +1,139 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+
+################################################################################
+# Copyright 2017 ROBOTIS CO., LTD.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+################################################################################
+
+# Author: Ryu Woon Jung (Leon)
+
+#
+# *********     Read and Write Example      *********
+#
+#
+# Available Dynamixel model on this example : All models using Protocol 2.0
+# This example is designed for using a Dynamixel PRO 54-200, and an USB2DYNAMIXEL.
+# To use another Dynamixel model, such as X series, see their details in E-Manual(emanual.robotis.com) and edit below variables yourself.
+# Be sure that Dynamixel PRO properties are already set as %% ID : 1 / Baudnum : 1 (Baudrate : 57600)
+#
+
+import os
+from dynamixel_sdk import *                    # Uses Dynamixel SDK library
+
+# Control table address
+ADDR_PRO_TORQUE_ENABLE      = 24               # Control table address is different in Dynamixel model
+ADDR_PRO_GOAL_POSITION      = 30
+ADDR_PRO_PRESENT_POSITION   = 37
+
+# Protocol version
+PROTOCOL_VERSION            = 2.0               # See which protocol version is used in the Dynamixel
+
+# Default setting
+DXL_ID                      = 13                 # Dynamixel ID : 1
+BAUDRATE                    = 1000000             # Dynamixel default baudrate : 57600
+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  = 205           # Dynamixel will rotate between this value
+DXL_MAXIMUM_POSITION_VALUE  = 819            # 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 cameraPortHandler instance
+# Set the port path
+# Get methods and members of cameraPortHandlerLinux or cameraPortHandlerWindows
+cameraPortHandler = PortHandler(DEVICENAME)
+
+# Initialize PacketHandler instance
+# Set the protocol version
+# Get methods and members of Protocol1PacketHandler or Protocol2PacketHandler
+packetHandler = PacketHandler(PROTOCOL_VERSION)
+
+# Open port
+if cameraPortHandler.openPort():
+    #print("Succeeded to open the port")
+    pass
+else:
+    print("Failed to open the port")
+    print("Press any key to terminate...")
+    getch()
+    quit()
+
+
+# Set port baudrate
+if cameraPortHandler.setBaudRate(BAUDRATE):
+    #print("Succeeded to change the baudrate")
+    pass
+else:
+    print("Failed to change the baudrate")
+    print("Press any key to terminate...")
+    getch()
+    quit()
+
+ID = 12
+goal_position = 500
+def cameraMode(ID):
+    # Enable Dynamixel Torque
+    dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(cameraPortHandler, 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:
+        pass
+        #print("Dynamixel has been successfully connected")
+
+cameraMode(12)
+cameraMode(13)
+def moveCamera(ID, goal_position):
+    #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(cameraPortHandler, ID, ADDR_PRO_GOAL_POSITION, goal_position)
+    if dxl_comm_result != COMM_SUCCESS:
+        print("111%s" % packetHandler.getTxRxResult(dxl_comm_result))
+    elif dxl_error != 0:
+        print("222%s" % packetHandler.getRxPacketError(dxl_error))
+
+    # Read present position
+    dxl_present_position, dxl_comm_result, dxl_error = packetHandler.read4ByteTxRx(cameraPortHandler, ID, ADDR_PRO_PRESENT_POSITION)
+    #if dxl_comm_result != COMM_SUCCESS:
+        #print("333%s" % packetHandler.getTxRxResult(dxl_comm_result))
+    if dxl_error != 0:
+        print("444%s" % packetHandler.getRxPacketError(dxl_error))
+
+    #print("555[ID:%03d] GoalPos:%03d  PresPos:%03d" % (DXL_ID, dxl_goal_position[index], dxl_present_position))
+
+    # if not abs(goal_position - dxl_present_position) > DXL_MOVING_STATUS_THRESHOLD:
+    #     print('Broken :(')
+
+
+# moveCamera(12, 1023)
+# moveCamera(13, 500)
+# # Disable Dynamixel Torque
+# dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(cameraPortHandler, DXL_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE)
+# if dxl_comm_result != COMM_SUCCESS:
+#     print("666%s" % packetHandler.getTxRxResult(dxl_comm_result))
+# elif dxl_error != 0:
+#     print("777%s" % packetHandler.getRxPacketError(dxl_error))
+
+# # Close port
+# cameraPortHandler.closePort()