From 9da0213a4a23c196af838a37351b8bfc152f4acd Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 17 Apr 2008 23:52:49 +0200 Subject: * digital/asserv/tools: - added support for new parameters. - added goto and goto_angle support. - added test_goto script. --- digital/asserv/tools/asserv.py | 31 +++++++++++++++++++++++++++++-- digital/asserv/tools/init.py | 9 +++++++++ digital/asserv/tools/test_goto.py | 22 ++++++++++++++++++++++ 3 files changed, 60 insertions(+), 2 deletions(-) create mode 100644 digital/asserv/tools/init.py create mode 100644 digital/asserv/tools/test_goto.py (limited to 'digital') 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 () -- cgit v1.2.3