/
motorctl.py
128 lines (109 loc) · 3.65 KB
/
motorctl.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
from __future__ import division
import atexit, sys, time
from louie import dispatcher
from twisted.internet import reactor
sys.path.append("/home/drewp/projects/light9/light9/io")
try:
import parport
parport.getparport()
except ImportError:
class _:
def outdata(self, *args):
pass
parport = _()
class Ctl:
def __init__(self):
self.blade = False
self.xpos = 0
self.ypos = 0
self.offTimer = None
self.lastPickup = None
dispatcher.connect(self.dragTo, "dragto")
self.path = [] # future points to walk to
self.lastByteTime = 0
atexit.register(lambda: self.out(0))
def resetPos(self):
self.xpos = self.ypos = 0
dispatcher.send("coords", x=self.xpos, y=self.ypos)
self.update()
def off(self):
self.out(0)
def dragTo(self, x, y, now=False, blade=False):
self.path.append((x, y, blade))
#print "drag to", x, y, len(self.path)
dispatcher.send("new path", path=self.path)
if now:
while self._step():
print "stepping now"
pass
def step(self, runFor=.05):
start = time.time()
more = True
while time.time() - start < .05 and more:
more = self._step()
return more
def _step(self):
if not self.path:
if self.offTimer is None or self.offTimer.called:
self.offTimer = reactor.callLater(.1, self.off)
return False
if self.offTimer is not None and not self.offTimer.called:
self.offTimer.reset(.1)
goal = self.path[0]
goal = int(goal[0]), int(goal[1]), goal[2]
#print "at %s, to %s, %s lines left" % (
# (self.xpos, self.ypos), goal, len(self.path))
self.setBlade(goal[2])
if (self.xpos, self.ypos) == goal[:2]:
self.path.pop(0)
dispatcher.send("new path", path=self.path)
return True
self.move(cmp(goal[0], self.xpos),
cmp(goal[1], self.ypos))
return True
def move(self, dx, dy):
if dx == dy == 0:
return
if abs(dx) > 1 or abs(dy) > 1:
print "========================= dx=%s dy=%s ======================" % (dx,dy)
self.xpos += dx
self.ypos += dy
dispatcher.send("coords", x=self.xpos, y=self.ypos)
#print "x=%d y=%d" % (self.xpos, self.ypos)
self.update()
def update(self):
byte = 0
if self.blade:
byte |= 0x80
byte |= (0x01, 0x03, 0x02, 0x00)[self.xpos % 4] * 0x20
byte |= (0x01, 0x03, 0x02, 0x00)[self.ypos % 4] * 0x04
byte |= 0x01 # power pin
byte |= 0x02 | 0x10 # enable dirs
now = time.time()
# print "%.1fms delay between bytes" % ((now - self.lastByteTime) * 1000)
self.out(byte)
self.lastByteTime = now
def out(self, byte):
#print hex(byte)
parport.outdata(byte)
# at .0014, Y loses steps
# at .002, Y still loses
time.sleep(.003)
def toggleBlade(self):
self.setBlade(not self.blade)
def setBlade(self, which):
if which == self.blade:
return
self.blade = which
if self.blade:
# blade needs full power to go down
self.out(0x80)
time.sleep(.05)
else:
self.out(0x00)
# blade-up has relay problems- it sticks
if time.time() - 10 > self.lastPickup:
time.sleep(.5)
self.lastPickup = time.time()
time.sleep(.1)
self.update()