From 2a5a4d11d59e2460746716d2a0a0ae697d434e57 Mon Sep 17 00:00:00 2001 From: Angus Finch Date: Tue, 9 Jul 2019 09:35:28 +1000 Subject: [PATCH] Add support for XL-320 Dynamixels This code is very messy, made in a rush during RoboCup 2019 --- Server/Server-Code-2019/GPIO.py | 3 +- .../NewRoboCupServer-Current.py | 77 ++++++++++ .../Server-Code-2019/RoboCupServer-Current.py | 5 +- .../__pycache__/GPIO.cpython-35.pyc | Bin 960 -> 678 bytes .../__pycache__/emuBot.cpython-35.pyc | Bin 3414 -> 2803 bytes .../__pycache__/xl320.cpython-35.pyc | Bin 0 -> 1808 bytes Server/Server-Code-2019/camera.py | 129 ++++++++++++++++ Server/Server-Code-2019/emuBot.py | 2 +- Server/Server-Code-2019/xl320.py | 139 ++++++++++++++++++ 9 files changed, 350 insertions(+), 5 deletions(-) create mode 100644 Server/Server-Code-2019/NewRoboCupServer-Current.py create mode 100644 Server/Server-Code-2019/__pycache__/xl320.cpython-35.pyc create mode 100644 Server/Server-Code-2019/camera.py create mode 100644 Server/Server-Code-2019/xl320.py diff --git a/Server/Server-Code-2019/GPIO.py b/Server/Server-Code-2019/GPIO.py index 66c35ae..eb60616 100644 --- 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 new file mode 100644 index 0000000..0c37d6d --- /dev/null +++ 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 index 8bb2c98..76e6f31 100644 --- 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 index 0328bf796b600e20f0758fd04eee6ddde73046bb..1a1bb7c4868987565c62b5bf75d0d2682a8f81c9 100644 GIT binary patch literal 678 zcmZuu%T60H6utJm(mV)ANZnOzMEIKHejBE!A+we2| z(rj7f7urpaohDROEc?#6kGa?9_-&(6efj-v7&D?5`e$+APdMrnCdFnnCc0&mu$9Xx z;grHUlw!N|M3lIcdX)MW127td!2&HPUhKkN8@O5yKpRIL!)V(bKg=01<8T{t*1k*H zvmu}6oEYvj2zlHIgVMd|cY8@q34K43d6wU(B!DxSrXq39+h0o$-mEB{BFht}clGU= zq>qu-{MwIxOeZ2LvgqQf8x4fK6Y}q;)kcff`@^G?$c#8B9?CZc*mkCaGKN@#Z8KQ` z+Dz{YGGH`ErDm2G0+j(XB@APS*}Z0XA=Rj#zGXY0xeNs>H;a0 z9!Xt;qqbm(1)yHF0#uo7A`<#Z<+7h07@h1W*+A6JS&lbF8@pFN;>BiTab~WsjGRqY J=RU7{e*i&^ewqLP literal 960 zcmb_a&2G~`5T5mK;v{ON3h@L;7%4R%goFeXkto2SPSvDBveZMUSE(V1&3Z%Oz$rWe zFTsho;30fM;zC}56W^>$NF|P}vYVOl?9BIlJJ&m%;Pcnu^qV01L0ij${RF!_#F64B zXh!r}P$GkiGHIr4G5YvPb~qD753jdKZ^NuX3Ipy%;8~QtFF|T4al9 znfP$#c`6h8Y45Od;Vp~A9-fT9kn(WL9KQ~t^L#F&B8&R6e53OsI?i9^&rkZ@`J~iR zMI))+ZT`AF6ztx;edj^M4fl%=Rg(=<++i|YnM6nI3e|x1Z^k$91teF<)PlK!IOs7k z4QXrEhN2TI)=A;PsIrD}=$NvKdsSY()WuR)ZlSV;{!6O1d`@YME;4;~K8&02N=8J& z2e?3}HthcdZ*Ab&!@*!1p9;`HwfNZRy#=j#32iXgLCRHZ9NA#w;w(as(;ne?f1lN> zhoygdlRiRCOb)9FDWN0J5JbEZc-x_ums6AdM7b#nVSPUcoyt{{#Z($aFeo!A3&jX{ zX0t8qMp1t>mOL2OkWey15_^y|z*c*F$xj-`N7+7)Mg2^U&gQayHrCvfHmO^sjX?E_ XJ;cy1XYw)AHVt+}V71-0*Y<4Jk@8|?hi<(4U-?qrJ%(zWWo7RLwo>v8hea)S}Wh`VB1tJTe0=F|6qQEj@*indljt@*K6~0n>`E!k} zdaxw2bbUNE7Wiq~0Qp2w0?T+IDyx;7l+;!CgAWjBUJ@ z&UvLsn}`@M#I#m+sz>&lk+SgOa>jEP9xpF%)Ycak7HhRQ-Bx~}<6^ts*pd2^dLWuo zdBshs*A7+>R%Nu?)ZQp=R}WT<%c4@Diu2;2xgpss+g_m&wpxj6oHm2slgeVXKV~Xh zVM|upe&yr!rAkezJ*m2vdkdkE_vYT6yvOBLy<#AmTSF+rt+{G zyazrUv^1JQ><1WtYulg%hyWLGIZe?Mn@%h`u}K3ui;gj9lQrP#ywnwV+6=q^?E&oH zdMiEv)W6Er1SMPTyn^pGQR%!2t8)jPe}f zt|0(#x=*Vt2}s_xT*+5E%>GUWT|yX+o_{vdC5$TYL=jy%VU6MdDom51h#KZ8N8R9o zG43uSLw-Ai6f7ex@53cxI&$)X0rs>tYG1T?Tus93a#nnYnSt>BA8BcFEmKvu@G%BD zHNn=u1K9qSluZU~4eyLxbkv2*K*RanV5Q=NW7k(KRM;1xfj)If;Lr)k0mZ_JI@?vO zK3^bW4n2WWnocgdb_KsPjTJg?pu3zAUQx-qc(1P+8N&@P*A63J`(Y5LqqdadIa$1$ ztS4<~i462c&+SD$$+yBi`6<)rD=gjB?}R|=b&T{Y9!pyvjURDyB~c*ZjzI$=3>^U) z-Li)$dZq;Ch*jq^7HFvbvnIar%?t(y(l5gfUF0vWHQaw^Hx+dOr3x-!=IvozGc4bnq|Ao$c%0IHOU8M<#liQ3O|K{pgL=z9kj;%q?0C8PhwG14 zAFeHWLo4U14C{zjOv-<+@^ApX9RCclr(%|SY5XQMw&K*YU0-`cC@Ayz(OJchP9j2F zKa5-`Q}~z<=EKgBh<_TCsZ6=|TcOx($`2UHXa=2Yxpu+MU~|}@?brpYU}deWJ#x-= MGwy_Y+jZUiU&Gw`kN^Mx literal 3414 zcmb7G&u`pB6n?h%_j;2wO&h4%ve1IGRg-NiI20jhlN7Z`*;Yv-B&$-`u1ATJU3)v8 zv`Lhlf&>RHT)1#YApRF_TzuhxIP8rx0;S&@?=QDbX`OiX&CK(>neV;ty_v04NMb4gW&p;@_Y)ku#JHatu9gQro15u0@VTB13M5jxd)cks~*&j`j+O|l3KnkcEq)ghfcpQM7Wl;P}bbuLC zYuy{u%8=4t&|2{0YRziwo9~Cxz{0g$WG~)dUS3~aTU@-ex*BD>((_eR={8&2Lfvip zZd=HxvLV#N-Id)H5$?2AG==WU?gOoIN6J7(MR&KoE?BIYPN@}iI(^lw9{h!53D*9( zUEd5kqTcoDOCsD>LASmVJPz)zEzNbBA&{!C3i)*SGq)JHV(!MZ>o@D7v$GJW`R<;! zq-}=h8Jz+eGHl}{G{jb9A33mG5gSb=e$)xgiu0anOr{cqCr^QH1Q0tFgeYyPr0F*X z!8H0HWJm(E7a{>%N!^KSz8MJLr|G{Mgp-2skF=$dASCdAjY!aWpxEdYL6BY!jdu)& z>|51-Ug$BeJOZIS0&{Bx$yACao=H!M8w-0%a5{*}ob>uhLNehbr>)2em6W`y%%iDU zk$G<^n&cG0nPD9%5?MsD37r<=rp#X!w*sV}W$^woy5RyEX7@$Bm+`)94y4QG>yOGa zSS39#+(foWM1W4A=@GERghZxb{p=tG=nR0=3;M*VcVkQoK8lIYS_v~)2s5dH_UqUJ zC!6_gOzR~xc`gqev;OxqAB|}x%;*5hH{=AM)s;k-lu7h@Z*ilGOB^+F zigiwLGYpYYbo68D`XHuimORa>Gu)(x$g@o$+8iPcsDu~MkPZ+o0kb+ll9GHI8Z9=> z4~ONjEc}R0!Y_vQYYdo0hZ#D6zvEJ{%!s3u_Dg{|CGXtDGXcm2qv?1s10j?vyC2~Z&(0KIk@$n#5n3E?`Fpea4V=WVt?>wu| zaFfbF_Wzg6LLVw2$K)6TEgPx1XinfaXI(TZLq^JXpjVrYGRo_SsHiqMI6Z&EsknQ7 zv*YcG_Il`UJB5YYYfCG)AKr1w55~$1ClIF+UpfaRhXczg@D-djb@6esxSX~&qs)^X zPdVjKs0CcFS8%=7*F#+kbVkr&s`oS&-+W7zx)K#W=>+ahTX4uycomIp*k;Mh;dD5m WX_+OXWaRNTIg(AgU|+G%*yZN{mR~ji diff --git a/Server/Server-Code-2019/__pycache__/xl320.cpython-35.pyc b/Server/Server-Code-2019/__pycache__/xl320.cpython-35.pyc new file mode 100644 index 0000000000000000000000000000000000000000..74dfc5413273d72c22aa208149b58cfb7067a2e9 GIT binary patch literal 1808 zcmaJ>-A?O95FXoc5)y|H;E)3p6t$vKDnWq5O{J*%cTPJ6#)^ zL~uplp|8+NeUqv$;M?{l_r2;>XPgk=w5shL&+g3Z&-cx2HkC?v`?+ubd;{;0wUx@F;&n{L#jl;GUPWoMG;U56SLx(X$=P z=Q}$)qmAdd&Xwg{@-Xr{>y`=h7;Ci3U$|h07NrMQ1QlBeo9CeL0j?$RWw=LO0Y3td z5x9`yL4pSyM3V|P3WzfJ$ToJ-KStvS(F(!jd=B@RA9Cn*{lY6w<1?=?rS!Z~AIV#u zpzY-6f>ht?+;IQIa#)v(VEzYT`+VItQ5=Trg_$En5{G$C+tq^3#T^&Z-*(Ame0zU) z@5{oV`)+@Cr#}o5lXKM2azVHgEg)}7nzTku7OU9rP%Y9!^vo#w6+0hMMA%O#LY5Q) z8A+Q`gk?FQDEbQg&)^=5cS6x6F^WY!B~qDDL<8}sOqhd51uhhLh)^2PGP4PO4kw8G zdjhO{J+IbB>fYYol;e@QzrVlbjnspKgDo$g5J^1Gl;?vdo&{A(@XYLSqj+kbHS}t$ z+Ng^KG>Qy~MG{Non-bh&v-n_jmK0~rie9O=X3=?7EQbM?uGKaBu4ntU>j=CCL9CEr znYisE(cJN$?L>0Ow4mY+5G-w0&a3sP+8SUr2d zyQar(#5`KWI*APw#sX$eb^B)xJ}(xRHGDVnUv T2`M4Pq*x^VtfL)0l4SV@kZqU` literal 0 HcmV?d00001 diff --git a/Server/Server-Code-2019/camera.py b/Server/Server-Code-2019/camera.py new file mode 100644 index 0000000..95c11ae --- /dev/null +++ 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 index 8a72317..97409de 100644 --- 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 new file mode 100644 index 0000000..52ce1cb --- /dev/null +++ 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()