-
Notifications
You must be signed in to change notification settings - Fork 0
/
disable_servo.py
167 lines (142 loc) · 6.45 KB
/
disable_servo.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
#!/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.
################################################################################
#*******************************************************************************
#*********************** Clear Multi Turn Example [Extended Position Control Mode] ***********************
# Required Environment to run this example :
# - Protocol 2.0 supported DYNAMIXEL(X, P, PRO/PRO(A), MX 2.0 series)
# - DYNAMIXEL Starter Set (U2D2, U2D2 PHB, 12V SMPS)
# How to use the example :
# - Select the DYNAMIXEL in use at the MY_DXL in the example code.
# - Note that the XL320 does support Extended Position Control Mode
# - Build and Run from proper architecture subdirectory.
# - For ARM based SBCs such as Raspberry Pi, use linux_sbc subdirectory to build and run.
# - https://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_sdk/overview/
# Author: Ki Jong Gil (Gilbert)
# Maintainer : Zerom, Will Son
# ****
from __future__ import print_function
import os
if os.name == 'nt':
import msvcrt
def getch():
return msvcrt.getch().decode()
def kbhit():
return msvcrt.kbhit()
else:
import termios, fcntl, sys, os
from select import select
fd = sys.stdin.fileno()
old_term = termios.tcgetattr(fd)
new_term = termios.tcgetattr(fd)
def getch():
new_term[3] = (new_term[3] & ~termios.ICANON & ~termios.ECHO)
termios.tcsetattr(fd, termios.TCSANOW, new_term)
try:
ch = sys.stdin.read(1)
finally:
termios.tcsetattr(fd, termios.TCSADRAIN, old_term)
return ch
def kbhit():
new_term[3] = (new_term[3] & ~(termios.ICANON | termios.ECHO))
termios.tcsetattr(fd, termios.TCSANOW, new_term)
try:
dr,dw,de = select([sys.stdin], [], [], 0)
if dr != []:
return 1
finally:
termios.tcsetattr(fd, termios.TCSADRAIN, old_term)
sys.stdout.flush()
return 0
from dynamixel_sdk import * # Uses Dynamixel SDK library
#********* DYNAMIXEL Model definition *********
#***** (Use only one definition at a time) *****
MY_DXL = 'X_SERIES' # X330 (5.0 V recommended), X430, X540, 2X430 (Note that XL320 does not support Extended Position Control Mode)
# MY_DXL = 'MX_SERIES' # MX series with 2.0 firmware update.
# MY_DXL = 'PRO_SERIES' # H54, H42, M54, M42, L54, L42
# MY_DXL = 'PRO_A_SERIES' # PRO series with (A) firmware update.
# MY_DXL = 'P_SERIES' # PH54, PH42, PM54
# Control table address
if MY_DXL == 'X_SERIES' or MY_DXL == 'MX_SERIES':
ADDR_TORQUE_ENABLE = 64
ADDR_GOAL_POSITION = 116
ADDR_PRESENT_POSITION = 132
elif MY_DXL == 'PRO_SERIES':
ADDR_TORQUE_ENABLE = 562 # Control table address is different in DYNAMIXEL model
ADDR_GOAL_POSITION = 596
ADDR_PRESENT_POSITION = 611
elif MY_DXL == 'P_SERIES' or MY_DXL == 'PRO_A_SERIES':
ADDR_TORQUE_ENABLE = 512 # Control table address is different in DYNAMIXEL model
ADDR_GOAL_POSITION = 564
ADDR_PRESENT_POSITION = 580
ADDR_OPERATING_MODE = 11
# Protocol version
PROTOCOL_VERSION = 2.0 # See which protocol version is used in the Dynamixel
# Factory default ID of all DYNAMIXEL is 1
DXL_ID = 1
# Use the actual port assigned to the U2D2.
# ex) Windows: "COM*", Linux: "/dev/ttyUSB*", Mac: "/dev/tty.usbserial-*"
DEVICENAME = '/dev/ttyUSB0'
BAUDRATE = 57600
EXT_POSITION_CONTROL_MODE = 4 # The value of Extended Position Control Mode that can be set through the Operating Mode (11)
TORQUE_ENABLE = 1 # Value for enabling the torque
TORQUE_DISABLE = 0 # Value for disabling the torque
MAX_POSITION_VALUE = 400000 # Of MX with firmware 2.0 and X-Series the revolution on Extended Position Control Mode is 256 rev
MIN_POSITION_VALUE = -1048575 # Of MX with firmware 2.0 and X-Series the revolution on Extended Position Control Mode is 256 rev
DXL_MOVING_STATUS_THRESHOLD = 20 # Dynamixel will rotate between this value
ESC_ASCII_VALUE = 0x1b
SPACE_ASCII_VALUE = 0x20
# 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()
# Set operating mode to extended position control mode
dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL_ID, ADDR_OPERATING_MODE, EXT_POSITION_CONTROL_MODE)
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("Operating mode changed to extended position control mode.")
# Disable Dynamixel Torque
dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL_ID, ADDR_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()