forked from limbryan/py-dynamixel
-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathexample.py
87 lines (70 loc) · 2.45 KB
/
example.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
import numpy as np
import time
import py_dynamixel.io as io
ports = io.get_available_ports()
print('available ports:', ports)
if not ports:
raise IOError('No port available.')
port = ports[0]
print('Using the first on the list', port)
dxl_io = io.DxlIO(port, baudrate=3000000, use_sync_write=True, use_sync_read=True)
print('Connected!')
#ids = list()
#for i in range(10, 70, 10):
# for j in range(1, 4):
# ids.append(i + j)
#print("ids", ids)
# found_ids = set()
# while len(found_ids) < 18:
# new_ids = dxl_io.scan(range(200))
# found_ids = set(new_ids).union(found_ids)
# print('Found ids:', found_ids)
ids = dxl_io.scan(range(200))
print(ids)
#ids = [2,3]
#print(ids)
dxl_io.init_sync_read(ids)
#dxl_io.enable_torque(ids)
#ANGLE_1 = 90
#dxl_io.set_goal_position(ids, np.array([ANGLE_1, ANGLE_1]), units="deg")
#time.sleep(2)
#dxl_io.set_goal_position(ids, np.array([45, 45]), units="deg")
'''
dxl_io.set_goal_position([2, 3], np.array([ANGLE_1, ANGLE_1]), units="deg")
time.sleep(2)
dxl_io.set_goal_position([2, 3], np.array([ANGLE_2, ANGLE_2]), units="deg")
time.sleep(2)
dxl_io.set_goal_position([2, 3], np.array([ANGLE_1, ANGLE_1]), units="deg")
time.sleep(2)
dxl_io.set_goal_position([2, 3], np.array([ANGLE_2, ANGLE_2]), units="deg")
time.sleep(2)
dxl_io.set_goal_position([2, 3], np.array([ANGLE_1, ANGLE_1]), units="deg")
time.sleep(2)
dxl_io.set_goal_position([2, 3], np.array([ANGLE_2, ANGLE_2]), units="deg")
'''
#time.sleep(2)
# dxl_io.set_goal_position(ids, np.array([180, 220, 220] * 6), units="deg")
# time.sleep(1.5)
# dxl_io.set_goal_position(ids, np.array([180, 180, 180] * 6), units="deg")
# time.sleep(1.5)
# dxl_io.set_goal_position(ids, np.array([180, 220, 220] * 6), units="deg")
# time.sleep(1.5)
# dxl_io.set_goal_position(ids, np.array([180, 180, 180] * 6), units="deg")
# time.sleep(1.5)
# dxl_io.set_goal_position(ids, np.array([180, 220, 220] * 6), units="deg")
# time.sleep(1.5)
# dxl_io.set_goal_position(ids, np.array([180, 180, 180] * 6), units="deg")
# time.sleep(1.5)
#while (1):
# cur_pos = dxl_io.get_present_position(ids)
# cur_vel = dxl_io.get_present_velocity(ids)
# if not np.all(cur_pos == 0):
# print("Current posiiton: ", cur_pos)
# print("Current velocity: ", cur_vel)
#time.sleep(1)
# # dxl_io.set_goal_position(ids, np.array([180, 0]))
# time.sleep(5)
# cur_pos = dxl_io.get_present_position(ids)
# print("Current posiiton: ", cur_pos)
#dxl_io.disable_torque(ids)
dxl_io.close_port()