summaryrefslogtreecommitdiff
path: root/digital/asserv
diff options
context:
space:
mode:
authorNicolas Schodet2008-04-17 23:52:49 +0200
committerNicolas Schodet2008-04-17 23:52:49 +0200
commit9da0213a4a23c196af838a37351b8bfc152f4acd (patch)
tree3b155c1b12de8dd7155237dd5712a078366c1a8f /digital/asserv
parentf5e35a1f0426803e6ef1c59251fc2a47e174c67d (diff)
* digital/asserv/tools:
- added support for new parameters. - added goto and goto_angle support. - added test_goto script.
Diffstat (limited to 'digital/asserv')
-rw-r--r--digital/asserv/tools/asserv.py31
-rw-r--r--digital/asserv/tools/init.py9
-rw-r--r--digital/asserv/tools/test_goto.py22
3 files changed, 60 insertions, 2 deletions
diff --git a/digital/asserv/tools/asserv.py b/digital/asserv/tools/asserv.py
index 548eb79e..8b0752ec 100644
--- a/digital/asserv/tools/asserv.py
+++ b/digital/asserv/tools/asserv.py
@@ -34,18 +34,23 @@ class Asserv:
def __init__ (self, file, **param):
self.proto = proto.Proto (file, time.time, 0.1)
+ self.seq = 0
+ self.seq_ack = 0
+ self.proto.register ('A', 'BB', self.handle_ack)
def make_handle (s):
return lambda *args: self.handle_stats (s, *args)
for (s, f) in self.stats_format.iteritems ():
self.proto.register (s, f, make_handle (s))
self.stats_enabled = None
self.param = dict (
+ scale = 1,
tkp = 0, tki = 0, tkd = 0,
akp = 0, aki = 0, akd = 0,
a0kp = 0, a0ki = 0, a0kd = 0,
- E = 1023, I = 1023, b = 15000,
+ E = 1023, I = 1023, D = 1023, b = 15000,
ta = 1, aa = 1, a0a = 1,
- tsm = 0, asm = 0, tss = 0, ass = 0, a0sm = 0, a0ss = 0
+ tsm = 0, asm = 0, tss = 0, ass = 0, a0sm = 0, a0ss = 0,
+ c = 1, f = 0x1000,
)
self.param.update (param)
self.send_param ()
@@ -118,6 +123,19 @@ class Asserv:
assert w == 'a0'
self.proto.send ('s', 'b', s)
+ def goto (self, x, y):
+ """Go to position."""
+ self.seq += 1
+ self.proto.send ('x', 'LLB', 256 * x / self.param['scale'],
+ 256 * y / self.param['scale'], self.seq)
+ self.wait (self.finished)
+
+ def goto_angle (self, a):
+ """Go to angle."""
+ self.seq += 1
+ self.proto.send ('x', 'LB', a * (1 << 24) / 360, self.seq)
+ self.wait (self.finished)
+
def send_param (self):
p = self.param
self.proto.send ('p', 'BHH', ord ('p'), p['tkp'] * 256,
@@ -131,12 +149,15 @@ class Asserv:
self.proto.send ('p', 'BH', ord ('d'), p['a0kd'] * 256)
self.proto.send ('p', 'BH', ord ('E'), p['E'])
self.proto.send ('p', 'BH', ord ('I'), p['I'])
+ self.proto.send ('p', 'BH', ord ('D'), p['D'])
self.proto.send ('p', 'BH', ord ('b'), p['b'])
self.proto.send ('p', 'BHH', ord ('a'), p['ta'] * 256,
p['aa'] * 256)
self.proto.send ('p', 'BH', ord ('a'), p['a0a'] * 256)
self.proto.send ('p', 'BBBBB', ord ('s'), p['tsm'], p['asm'],
p['tss'], p['ass'])
+ self.proto.send ('p', 'BL', ord ('c'), p['c'] * 256 * 256 * 256)
+ self.proto.send ('p', 'BH', ord ('f'), p['f'])
def handle_stats (self, stat, *args):
if self.stats_enabled is not None:
@@ -146,6 +167,9 @@ class Asserv:
self.stats_line = [ ]
self.stats_count += 1
+ def handle_ack (self, mseq, a0seq):
+ self.seq_ack = mseq & 0x7f
+
def wait (self, cond = None):
try:
cond_count = int (cond)
@@ -154,6 +178,9 @@ class Asserv:
pass
self.proto.wait (cond)
+ def finished (self):
+ return self.seq == self.seq_ack
+
def reset (self):
self.proto.send ('w')
self.proto.send ('z')
diff --git a/digital/asserv/tools/init.py b/digital/asserv/tools/init.py
new file mode 100644
index 00000000..7d0fb79a
--- /dev/null
+++ b/digital/asserv/tools/init.py
@@ -0,0 +1,9 @@
+"""Default parameters for asserv."""
+host = dict (
+ scale = 0.0395840674352314, f = 0xdd1,
+ tkp = 1, tkd = 16, akp = 2, akd = 16,
+ a0p = 1, a0d = 16,
+ E = 0x3ff, D = 0x1ff,
+ ta = 0.5, aa = 0.5, tsm = 0x60, tss = 0x40, asm = 0x60, ass = 0x40,
+ a0a = 0.5, a0sm = 0x10, a0ss = 0x10,
+ )
diff --git a/digital/asserv/tools/test_goto.py b/digital/asserv/tools/test_goto.py
new file mode 100644
index 00000000..63a8cdf3
--- /dev/null
+++ b/digital/asserv/tools/test_goto.py
@@ -0,0 +1,22 @@
+import sys
+sys.path.append (sys.path[0] + '/../../../host/proto')
+
+from asserv import Asserv
+import init
+import popen_io
+import serial
+import random
+
+if sys.argv[1] == '!':
+ io = popen_io.PopenIO (sys.argv[2:])
+else:
+ io = serial.Serial (sys.argv[1])
+a = Asserv (io, **init.host)
+for i in xrange (10):
+ x = random.randrange (2000)
+ y = random.randrange (1100)
+ a.goto (x, y)
+ a.goto_angle (random.randrange (360))
+a.goto (0, 0)
+a.goto_angle (0)
+a.close ()