/
test.parallel.py
138 lines (112 loc) · 3.62 KB
/
test.parallel.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
# coding=utf-8
from __future__ import print_function
import RPi.GPIO as GPIO
import threading
import sys
sys.path.insert(0, '/home/pi/dynamixel_hr')
from dxl.dxlchain import DxlChain
chain = DxlChain("/dev/ttyUSB0", rate=1000000)
print(chain.get_motor_list())
class Pair(object):
"""
A quadrature encoder paired with one Dynamixel
"""
counter = 0
currentA = 1
currentB = 1
motor = 0
lockRotary = threading.Lock()
encoderA = 0
encoderB = 0
position = 0
initialPosition = 0
direction = 0
def sanity_check(self, new_position):
"""
Makes sure new position is in bounds
:param new_position: Raw displacement value from encoder
:return: Burns up the CPU
"""
self.position += new_position * abs(new_position) * 13
if self.position > 1000:
self.position = 1000
elif self.position < 0:
self.position = 0
else:
return
def rotary_interrupt(self, a_or_b):
"""
Interrupt callback
Gets called when a change in voltage is detected
:param a_or_b: Which pin called back
"""
switch_a = GPIO.input(self.encoderA)
switch_b = GPIO.input(self.encoderB)
if self.currentA == switch_a and self.currentB == switch_b:
return
self.currentA = switch_a
self.currentB = switch_b
if switch_a and switch_b:
self.lockRotary.acquire()
if a_or_b == self.encoderB:
self.counter += 1
else:
self.counter -= 1
self.lockRotary.release()
return
def __init__(self, encoderA, encoderB, motor, initialPosition, direction):
"""
Initializes an encoder and a Dynamixel
:param encoderA: Encoder pin A
:param encoderB: Encoder pin B
:param motor: Dynamixel ID number
:param initialPosition: Starting position of Dynamixel
:param direction: Direction that Dynamixel should turn
"""
self.encoderA = encoderA
self.encoderB = encoderB
self.motor = motor
self.initialPosition = initialPosition
self.direction = direction
GPIO.setwarnings(True)
GPIO.setmode(GPIO.BOARD)
GPIO.setup(self.encoderA, GPIO.IN)
GPIO.setup(self.encoderB, GPIO.IN)
GPIO.add_event_detect(self.encoderA, GPIO.RISING, callback=self.rotary_interrupt)
GPIO.add_event_detect(self.encoderB, GPIO.RISING, callback=self.rotary_interrupt)
def startup(self):
"""
Wiggles the finger
"""
chain.goto(self.motor, self.initialPosition, 1023, True)
chain.goto(self.motor, self.initialPosition + (250 * self.direction), 1023, True)
chain.goto(self.motor, self.initialPosition, 1023, True)
def update(pair):
"""
Update an Encoder's position value.
"""
pair.lockRotary.acquire()
new_position = pair.counter
pair.counter = 0
pair.lockRotary.release()
if new_position != 0:
pair.sanity_check(new_position)
#print(pair.encoderA, pair.encoderB, pair.motor, pair.position)
chain.goto(pair.motor, pair.position, 1000, False)
Pair1 = Pair(8, 10, 5, 0, 1)
Pair2 = Pair(11, 12, 6, 0, 1)
Pair3 = Pair(15, 16, 2, 0, 1)
Pair4 = Pair(21, 22, 3, 0, 1)
Pair5 = Pair(23, 24, 1, 0, 1)
Pair6 = Pair(31, 32, 4, 0, 1)
array = [Pair1, Pair2, Pair3, Pair4, Pair5, Pair6]
for Pair in array:
Pair.startup()
while True:
try:
for Pair in array:
update(Pair)
except KeyboardInterrupt:
GPIO.cleanup()
print("Bye! \n")
sys.exit(0)