-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathmobilityMovementCommandv3.py
234 lines (189 loc) · 8.53 KB
/
mobilityMovementCommandv3.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
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
from roboclaw_3 import Roboclaw
import time
import os
from dynamixel_sdk import * # Uses Dynamixel SDK library
class moveRover:
def init_RoboClaw():
address = 0x80
#Top right USB Port
rc1 = Roboclaw("/dev/ttyACM0",115200)
#Top Left USB Port
rc2 = Roboclaw("/dev/ttyACM1",115200)
rc1.Open()
rc2.Open()
version = rc1.ReadVersion(address)
if version[0]==False:
print("GETVERSION Failed")
else:
print(repr(version[1]))
version = rc2.ReadVersion(address)
if version[0]==False:
print("GETVERSION Failed")
else:
print(repr(version[1]))
return(rc1, rc2)
#emergency stop
def estop(rc1, rc2, portHandler, packetHandler):
address = 0x80
rc1.BackwardM1(address, 0)
rc1.BackwardM2(address, 0)
rc2.BackwardM1(address, 0)
rc2.BackwardM2(address, 0)
#control servo movement
DXL_ID1 = 1
DXL_ID2 = 2
DXL_ID3 = 3
DXL_ID4 = 4
ADDR_TORQUE_ENABLE = 64
TORQUE_ENABLE = 0 # Value for enabling the torque
TORQUE_DISABLE = 1 # Value for disabling the torque
# Disable Dynamixel Torque
dxl_comm_result1, dxl_error1 = packetHandler.write1ByteTxRx(portHandler, DXL_ID1, ADDR_TORQUE_ENABLE, TORQUE_DISABLE)
dxl_comm_result2, dxl_error2 = packetHandler.write1ByteTxRx(portHandler, DXL_ID2, ADDR_TORQUE_ENABLE, TORQUE_DISABLE)
dxl_comm_result1, dxl_error1 = packetHandler.write1ByteTxRx(portHandler, DXL_ID3, ADDR_TORQUE_ENABLE, TORQUE_DISABLE)
dxl_comm_result2, dxl_error2 = packetHandler.write1ByteTxRx(portHandler, DXL_ID4, ADDR_TORQUE_ENABLE, TORQUE_DISABLE)
#Wheel motors are on Node 1 Channel 1
def wheelMotors(rc1, vel):
address = 0x80
#value from 0 - 127
#64 is stop, 0 is full reverse, 127 is full forward
rc1.ForwardBackwardM1(address, vel)
return(1)
#Digging Motor is on Node 1 Channel 2
def digMotor(rc1, vel):
address = 0x80
rc1.ForwardBackwardM2(address, vel)
return(1)
#Digging Actuator is on Node 2 channel 1
def digActuator(rc2, vel):
address = 0x80
speed = 32
if vel>0:
rc2.ForwardM1(address,speed)
elif vel<0:
rc2.BackwardM1(address, speed)
else:
rc2.BackwardM1(address, 0)
return(1)
#Dump Actuators are Node 2 channel 2
def dumpActuator(rc2, vel):
address = 0x80
speed = 32
if vel>0:
rc2.ForwardM2(address, speed)
elif vel<0:
rc2.BackwardM2(address, speed)
else:
rc2.BackwardM2(address, 0)
return(1)
def servoSetup():
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
'''
#********* DYNAMIXEL Model definition *********
#***** (Use only one definition at a time) *****
MY_DXL = 'X_SERIES' # X330 (5.0 V recommended), X430, X540, 2X430
# Control table address
ADDR_TORQUE_ENABLE = 64
ADDR_GOAL_POSITION = 116
ADDR_PRESENT_POSITION = 132
DXL_MINIMUM_POSITION_VALUE = 0 # Refer to the Minimum Position Limit of product eManual
DXL_MAXIMUM_POSITION_VALUE = 4095 # Refer to the Maximum Position Limit of product eManual
BAUDRATE = 57600
# DYNAMIXEL Protocol Version (1.0 / 2.0)
# https://emanual.robotis.com/docs/en/dxl/protocol2/
PROTOCOL_VERSION = 2.0
# Factory default ID of all DYNAMIXEL is 1
DXL_ID1 = 1
DXL_ID2 = 2
# Use the actual port assigned to the U2D2.
# ex) Windows: "COM*", Linux: "/dev/ttyUSB*", Mac: "/dev/tty.usbserial-*"
DEVICENAME = '/dev/ttyUSB0'
TORQUE_ENABLE = 1 # Value for enabling the torque
TORQUE_DISABLE = 0 # Value for disabling the torque
DXL_MOVING_STATUS_THRESHOLD = 20 # Dynamixel moving status threshold
index = 1
angle = 2000
dxl_goal_position = [1000, 2000] # 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()
#get started
#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_result1, dxl_error1 = packetHandler.write1ByteTxRx(portHandler, DXL_ID1, ADDR_TORQUE_ENABLE, TORQUE_ENABLE)
dxl_comm_result2, dxl_error2 = packetHandler.write1ByteTxRx(portHandler, DXL_ID2, ADDR_TORQUE_ENABLE, TORQUE_ENABLE)
if dxl_comm_result1 != COMM_SUCCESS:
print("%s" % packetHandler.getTxRxResult(dxl_comm_result1))
elif dxl_error1 != 0:
print("%s" % packetHandler.getRxPacketError(dxl_error1))
else:
print("Dynamixel has been successfully connected")
return(portHandler, packetHandler)
def moveServos(portHandler, packetHandler, s1, s2, s3, s4):
#control servo movement
DXL_ID1 = 1
DXL_ID2 = 2
DXL_ID3 = 3
DXL_ID4 = 4
ADDR_GOAL_POSITION = 116
ADDR_TORQUE_ENABLE = 64
TORQUE_ENABLE = 1 # Value for enabling the torque
TORQUE_DISABLE = 0 # Value for disabling the torque
# Enable Dynamixel Torque
dxl_comm_result1, dxl_error1 = packetHandler.write1ByteTxRx(portHandler, DXL_ID1, ADDR_TORQUE_ENABLE, TORQUE_ENABLE)
dxl_comm_result2, dxl_error2 = packetHandler.write1ByteTxRx(portHandler, DXL_ID2, ADDR_TORQUE_ENABLE, TORQUE_ENABLE)
dxl_comm_result1, dxl_error1 = packetHandler.write1ByteTxRx(portHandler, DXL_ID3, ADDR_TORQUE_ENABLE, TORQUE_ENABLE)
dxl_comm_result2, dxl_error2 = packetHandler.write1ByteTxRx(portHandler, DXL_ID4, ADDR_TORQUE_ENABLE, TORQUE_ENABLE)
# Write goal position
dxl_comm_result1, dxl_error1 = packetHandler.write4ByteTxRx(portHandler, DXL_ID1, ADDR_GOAL_POSITION, s1)
dxl_comm_result2, dxl_error2 = packetHandler.write4ByteTxRx(portHandler, DXL_ID2, ADDR_GOAL_POSITION, s2)
dxl_comm_result1, dxl_error1 = packetHandler.write4ByteTxRx(portHandler, DXL_ID3, ADDR_GOAL_POSITION, s3)
dxl_comm_result2, dxl_error2 = packetHandler.write4ByteTxRx(portHandler, DXL_ID4, ADDR_GOAL_POSITION, s4)
# Disable Dynamixel Torque
dxl_comm_result1, dxl_error1 = packetHandler.write1ByteTxRx(portHandler, DXL_ID1, ADDR_TORQUE_ENABLE, TORQUE_DISABLE)
dxl_comm_result2, dxl_error2 = packetHandler.write1ByteTxRx(portHandler, DXL_ID2, ADDR_TORQUE_ENABLE, TORQUE_DISABLE)
dxl_comm_result1, dxl_error1 = packetHandler.write1ByteTxRx(portHandler, DXL_ID3, ADDR_TORQUE_ENABLE, TORQUE_DISABLE)
dxl_comm_result2, dxl_error2 = packetHandler.write1ByteTxRx(portHandler, DXL_ID4, ADDR_TORQUE_ENABLE, TORQUE_DISABLE)