From 2e95e3a33bcb34aeec66551503c692c1cb80ab61 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 9 Feb 2009 20:05:30 +0100 Subject: * all python: - changed tabs to spaces. --- digital/asserv/tools/asserv/asserv.py | 422 +++++++++++++------------- digital/asserv/tools/asserv/init.py | 30 +- digital/asserv/tools/inter_asserv.py | 126 ++++---- digital/asserv/tools/step.py | 4 +- digital/avr/modules/path/test/test_path.py | 182 +++++------ digital/avr/modules/twi/test/test_twi_host.py | 4 +- digital/io/tools/io/init.py | 8 +- digital/io/tools/io/io.py | 40 +-- digital/io/tools/test_simu.py | 76 ++--- host/inter/dist_sensor.py | 88 +++--- host/inter/drawable.py | 176 +++++------ host/inter/inter.py | 380 +++++++++++------------ host/inter/inter_node.py | 266 ++++++++-------- host/inter/path.py | 12 +- host/inter/test/test_drawable.py | 48 +-- host/inter/trans_matrix.py | 226 +++++++------- host/mex/hub.py | 268 ++++++++-------- host/mex/msg.py | 96 +++--- host/mex/node.py | 188 ++++++------ host/mex/test/test.py | 20 +- host/proto/popen_io.py | 22 +- host/proto/proto.py | 228 +++++++------- host/proto/test/fio.py | 24 +- host/proto/test/interactive.py | 2 +- host/utils/forked.py | 18 +- tools/dfagen/dfagen/automaton.py | 142 ++++----- tools/dfagen/dfagen/command.py | 8 +- tools/dfagen/dfagen/output/__init__.py | 30 +- tools/dfagen/dfagen/output/c.py | 260 ++++++++-------- tools/dfagen/dfagen/output/dot.py | 22 +- tools/trace/lib/traceclass.py | 6 +- 31 files changed, 1711 insertions(+), 1711 deletions(-) diff --git a/digital/asserv/tools/asserv/asserv.py b/digital/asserv/tools/asserv/asserv.py index 05a07130..2424a88b 100644 --- a/digital/asserv/tools/asserv/asserv.py +++ b/digital/asserv/tools/asserv/asserv.py @@ -5,264 +5,264 @@ import math class Asserv: stats_format = { - 'C': 'HHH', - 'Z': 'H', - 'S': 'bbb', - 'P': 'hhhhhh', - 'W': 'hhh', - } + 'C': 'HHH', + 'Z': 'H', + 'S': 'bbb', + 'P': 'hhhhhh', + 'W': 'hhh', + } # The last occuring stats will increment stats_count, so they have to # be in the same order than in asserv program. stats_order = 'CZSPW' stats_items = { - 'lc': ('C', 0), - 'rc': ('C', 1), - 'a0c': ('C', 2), - 'a0z': ('Z', 0), - 'ts': ('S', 0), - 'as': ('S', 1), - 'a0s': ('S', 2), - 'te': ('P', 0), - 'ti': ('P', 1), - 'ae': ('P', 2), - 'ai': ('P', 3), - 'a0e': ('P', 4), - 'a0i': ('P', 5), - 'lw': ('W', 0), - 'rw': ('W', 1), - 'a0w': ('W', 2), - } + 'lc': ('C', 0), + 'rc': ('C', 1), + 'a0c': ('C', 2), + 'a0z': ('Z', 0), + 'ts': ('S', 0), + 'as': ('S', 1), + 'a0s': ('S', 2), + 'te': ('P', 0), + 'ti': ('P', 1), + 'ae': ('P', 2), + 'ai': ('P', 3), + 'a0e': ('P', 4), + 'a0i': ('P', 5), + 'lw': ('W', 0), + 'rw': ('W', 1), + 'a0w': ('W', 2), + } def __init__ (self, file, time = time.time, **param): - self.proto = proto.Proto (file, time, 0.1) - self.async = False - self.mseq = 0 - self.mseq_ack = 0 - self.a0seq = 0 - self.a0seq_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, D = 1023, - be = 2048, bs = 0x10, bc = 20, - ta = 1, aa = 1, a0a = 1, - tsm = 0, asm = 0, tss = 0, ass = 0, a0sm = 0, a0ss = 0, - c = 1, f = 0x1000, - l = 0x2000, - ) - self.param.update (param) - self.send_param () + self.proto = proto.Proto (file, time, 0.1) + self.async = False + self.mseq = 0 + self.mseq_ack = 0 + self.a0seq = 0 + self.a0seq_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, D = 1023, + be = 2048, bs = 0x10, bc = 20, + ta = 1, aa = 1, a0a = 1, + tsm = 0, asm = 0, tss = 0, ass = 0, a0sm = 0, a0ss = 0, + c = 1, f = 0x1000, + l = 0x2000, + ) + self.param.update (param) + self.send_param () def stats (self, *stats_items, **options): - """Activate stats.""" - interval = 1 - if 'interval' in options: - interval = options['interval'] - # Build list of stats letters. - stats = [self.stats_items[i][0] for i in stats_items] - stats = [s for s in self.stats_order if s in stats] - stats_last_pos = 0 - stats_pos = { } - for s in stats: - stats_pos[s] = stats_last_pos - stats_last_pos += len (self.stats_format[s]) - # Build stats item positions. - self.stats_items_pos = [ ] - for i in stats_items: - id = self.stats_items[i] - self.stats_items_pos.append (stats_pos[id[0]] + id[1]) - # Enable stats. - for s in stats: - self.proto.send (s, 'B', interval) - # Prepare aquisition. - self.stats_enabled = stats - self.stats_counter = stats[-1] - self.stats_count = 0 - self.stats_list = [ ] - self.stats_line = [ ] + """Activate stats.""" + interval = 1 + if 'interval' in options: + interval = options['interval'] + # Build list of stats letters. + stats = [self.stats_items[i][0] for i in stats_items] + stats = [s for s in self.stats_order if s in stats] + stats_last_pos = 0 + stats_pos = { } + for s in stats: + stats_pos[s] = stats_last_pos + stats_last_pos += len (self.stats_format[s]) + # Build stats item positions. + self.stats_items_pos = [ ] + for i in stats_items: + id = self.stats_items[i] + self.stats_items_pos.append (stats_pos[id[0]] + id[1]) + # Enable stats. + for s in stats: + self.proto.send (s, 'B', interval) + # Prepare aquisition. + self.stats_enabled = stats + self.stats_counter = stats[-1] + self.stats_count = 0 + self.stats_list = [ ] + self.stats_line = [ ] def get_stats (self, wait = None): - if wait: - self.wait (wait) - list = self.stats_list - # Drop first line as it might be garbage. - del list[0] - for s in reversed (self.stats_enabled): - self.proto.send (s, 'B', 0) - # Extract asked stats. - array = numpy.array (list) - array = array[:, self.stats_items_pos] - # Cleanup. - self.stats_enabled = None - del self.stats_items_pos - del self.stats_counter - del self.stats_count - del self.stats_list - del self.stats_line - return array + if wait: + self.wait (wait) + list = self.stats_list + # Drop first line as it might be garbage. + del list[0] + for s in reversed (self.stats_enabled): + self.proto.send (s, 'B', 0) + # Extract asked stats. + array = numpy.array (list) + array = array[:, self.stats_items_pos] + # Cleanup. + self.stats_enabled = None + del self.stats_items_pos + del self.stats_counter + del self.stats_count + del self.stats_list + del self.stats_line + return array def consign (self, w, c): - """Consign offset.""" - if w == 't': - self.proto.send ('c', 'hh', c, 0) - elif w == 'a': - self.proto.send ('c', 'hh', 0, c) - else: - assert w == 'a0' - self.proto.send ('c', 'h', c) + """Consign offset.""" + if w == 't': + self.proto.send ('c', 'hh', c, 0) + elif w == 'a': + self.proto.send ('c', 'hh', 0, c) + else: + assert w == 'a0' + self.proto.send ('c', 'h', c) def speed (self, w, s): - """Speed consign.""" - if w == 't': - self.proto.send ('s', 'bb', s, 0) - elif w == 'a': - self.proto.send ('s', 'bb', 0, s) - else: - assert w == 'a0' - self.proto.send ('s', 'b', s) + """Speed consign.""" + if w == 't': + self.proto.send ('s', 'bb', s, 0) + elif w == 'a': + self.proto.send ('s', 'bb', 0, s) + else: + assert w == 'a0' + self.proto.send ('s', 'b', s) def speed_pos (self, w, offset): - """Speed controlled position consign.""" - if w == 't': - self.mseq += 1 - self.proto.send ('s', 'LLB', offset, 0, self.mseq) - elif w == 'a': - self.mseq += 1 - self.proto.send ('s', 'LLB', 0, offset, self.mseq) - else: - assert w == 'a0' - self.a0seq += 1 - self.proto.send ('s', 'LB', offset, self.a0seq) - self.wait (self.finished, auto = True) + """Speed controlled position consign.""" + if w == 't': + self.mseq += 1 + self.proto.send ('s', 'LLB', offset, 0, self.mseq) + elif w == 'a': + self.mseq += 1 + self.proto.send ('s', 'LLB', 0, offset, self.mseq) + else: + assert w == 'a0' + self.a0seq += 1 + self.proto.send ('s', 'LB', offset, self.a0seq) + self.wait (self.finished, auto = True) def set_pos (self, x = None, y = None, a = None): - """Set current position.""" - if x is not None: - self.proto.send ('p', 'BL', ord ('X'), - 256 * x / self.param['scale']) - if y is not None: - self.proto.send ('p', 'BL', ord ('Y'), - 256 * y / self.param['scale']) - if a is not None: - self.proto.send ('p', 'BL', ord ('A'), a * (1 << 24) / 360) + """Set current position.""" + if x is not None: + self.proto.send ('p', 'BL', ord ('X'), + 256 * x / self.param['scale']) + if y is not None: + self.proto.send ('p', 'BL', ord ('Y'), + 256 * y / self.param['scale']) + if a is not None: + self.proto.send ('p', 'BL', ord ('A'), a * (1 << 24) / 360) def goto (self, x, y, backward_ok = False): - """Go to position.""" - self.mseq += 1 - self.proto.send (backward_ok and 'r' or 'x', 'LLB', - 256 * x / self.param['scale'], - 256 * y / self.param['scale'], self.mseq) - self.wait (self.finished, auto = True) + """Go to position.""" + self.mseq += 1 + self.proto.send (backward_ok and 'r' or 'x', 'LLB', + 256 * x / self.param['scale'], + 256 * y / self.param['scale'], self.mseq) + self.wait (self.finished, auto = True) def goto_angle (self, a): - """Go to angle.""" - self.mseq += 1 - self.proto.send ('x', 'HB', a * (1 << 16) / 360, self.mseq) - self.wait (self.finished, auto = True) + """Go to angle.""" + self.mseq += 1 + self.proto.send ('x', 'HB', a * (1 << 16) / 360, self.mseq) + self.wait (self.finished, auto = True) def goto_xya (self, x, y, a, backward_ok = False): - """Go to position, then angle.""" - self.mseq += 1 - self.proto.send (backward_ok and 'r' or 'x', 'LLHB', - 256 * x / self.param['scale'], - 256 * y / self.param['scale'], - a * (1 << 16) / 360, self.mseq) - self.wait (self.finished, auto = True) + """Go to position, then angle.""" + self.mseq += 1 + self.proto.send (backward_ok and 'r' or 'x', 'LLHB', + 256 * x / self.param['scale'], + 256 * y / self.param['scale'], + a * (1 << 16) / 360, self.mseq) + self.wait (self.finished, auto = True) def set_simu_pos (self, x, y, a): - """Set simulated position.""" - self.proto.send ('h', 'BHHH', ord ('X'), x, y, - math.radians (a) * 1024) + """Set simulated position.""" + self.proto.send ('h', 'BHHH', ord ('X'), x, y, + math.radians (a) * 1024) def register_pos (self, func, interval = 225 / 4): - """Will call func each time a position is received.""" - self.pos_func = func - self.proto.register ('X', 'lll', self.handle_pos) - self.proto.send ('X', 'B', interval) + """Will call func each time a position is received.""" + self.pos_func = func + self.proto.register ('X', 'lll', self.handle_pos) + self.proto.send ('X', 'B', interval) def send_param (self): - p = self.param - self.proto.send ('p', 'BHH', ord ('p'), p['tkp'] * 256, - p['akp'] * 256) - self.proto.send ('p', 'BHH', ord ('i'), p['tki'] * 256, - p['aki'] * 256) - self.proto.send ('p', 'BHH', ord ('d'), p['tkd'] * 256, - p['akd'] * 256) - self.proto.send ('p', 'BH', ord ('p'), p['a0kp'] * 256) - self.proto.send ('p', 'BH', ord ('i'), p['a0ki'] * 256) - 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', 'BHHH', ord ('b'), p['be'], p['bs'], p['bc']) - 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', 'BBB', ord ('s'), p['a0sm'], p['a0ss']) - self.proto.send ('p', 'BL', ord ('c'), p['c'] * 256 * 256 * 256) - self.proto.send ('p', 'BH', ord ('f'), p['f']) - self.proto.send ('p', 'BH', ord ('l'), p['l']) + p = self.param + self.proto.send ('p', 'BHH', ord ('p'), p['tkp'] * 256, + p['akp'] * 256) + self.proto.send ('p', 'BHH', ord ('i'), p['tki'] * 256, + p['aki'] * 256) + self.proto.send ('p', 'BHH', ord ('d'), p['tkd'] * 256, + p['akd'] * 256) + self.proto.send ('p', 'BH', ord ('p'), p['a0kp'] * 256) + self.proto.send ('p', 'BH', ord ('i'), p['a0ki'] * 256) + 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', 'BHHH', ord ('b'), p['be'], p['bs'], p['bc']) + 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', 'BBB', ord ('s'), p['a0sm'], p['a0ss']) + self.proto.send ('p', 'BL', ord ('c'), p['c'] * 256 * 256 * 256) + self.proto.send ('p', 'BH', ord ('f'), p['f']) + self.proto.send ('p', 'BH', ord ('l'), p['l']) def write_eeprom (self): - self.proto.send ('p', 'BB', ord ('E'), 1) - time.sleep (1) - self.wait (lambda: True) + self.proto.send ('p', 'BB', ord ('E'), 1) + time.sleep (1) + self.wait (lambda: True) def handle_stats (self, stat, *args): - if self.stats_enabled is not None: - self.stats_line.extend (args) - if self.stats_counter == stat: - self.stats_list.append (self.stats_line) - self.stats_line = [ ] - self.stats_count += 1 + if self.stats_enabled is not None: + self.stats_line.extend (args) + if self.stats_counter == stat: + self.stats_list.append (self.stats_line) + self.stats_line = [ ] + self.stats_count += 1 def handle_ack (self, mseq, a0seq): - self.mseq_ack = mseq & 0x7f - self.a0seq_ack = a0seq & 0x7f - self.proto.send ('a', 'BB', mseq, a0seq) + self.mseq_ack = mseq & 0x7f + self.a0seq_ack = a0seq & 0x7f + self.proto.send ('a', 'BB', mseq, a0seq) def handle_pos (self, x, y, a): - x = x / 256 * self.param['scale'] - y = y / 256 * self.param['scale'] - a = a * 360 / (1 << 24) - self.pos_func (x, y, a) + x = x / 256 * self.param['scale'] + y = y / 256 * self.param['scale'] + a = a * 360 / (1 << 24) + self.pos_func (x, y, a) def wait (self, cond = None, auto = False): - if auto and self.async: - return - try: - cond_count = int (cond) - cond = lambda: self.stats_count > cond_count - except TypeError: - pass - self.proto.wait (cond) + if auto and self.async: + return + try: + cond_count = int (cond) + cond = lambda: self.stats_count > cond_count + except TypeError: + pass + self.proto.wait (cond) def finished (self): - return self.mseq == self.mseq_ack and self.a0seq == self.a0seq_ack + return self.mseq == self.mseq_ack and self.a0seq == self.a0seq_ack def free (self): - self.proto.send ('w') + self.proto.send ('w') def reset (self): - self.proto.send ('w') - self.proto.send ('w', 'H', 0) - self.proto.send ('z') - self.proto.send ('z') + self.proto.send ('w') + self.proto.send ('w', 'H', 0) + self.proto.send ('z') + self.proto.send ('z') def close (self): - self.reset () - self.wait (lambda: True) - self.proto.file.close () + self.reset () + self.wait (lambda: True) + self.proto.file.close () def fileno (self): - return self.proto.fileno () + return self.proto.fileno () diff --git a/digital/asserv/tools/asserv/init.py b/digital/asserv/tools/asserv/init.py index 42c34f5d..8e4c8d5d 100644 --- a/digital/asserv/tools/asserv/init.py +++ b/digital/asserv/tools/asserv/init.py @@ -1,18 +1,18 @@ """Default parameters for asserv.""" host = dict ( - scale = 0.0395840674352314, f = 0xdd1, - tkp = 1, tkd = 16, akp = 2, akd = 16, - a0kp = 1, a0kd = 16, - E = 0x3ff, D = 0x1ff, - ta = 0.5, aa = 0.5, tsm = 0x60, tss = 0x20, asm = 0x30, ass = 0x10, - a0a = 0.5, a0sm = 0x0a, a0ss = 0x05, - ) + scale = 0.0395840674352314, f = 0xdd1, + tkp = 1, tkd = 16, akp = 2, akd = 16, + a0kp = 1, a0kd = 16, + E = 0x3ff, D = 0x1ff, + ta = 0.5, aa = 0.5, tsm = 0x60, tss = 0x20, asm = 0x30, ass = 0x10, + a0a = 0.5, a0sm = 0x0a, a0ss = 0x05, + ) target = dict ( - scale = 0.0413530725332892, f = 0xcfa, - c = float (0xffefbe) / (1 << 24), - tkp = 1, tkd = 16, akp = 2, akd = 16, - a0kp = 1, a0kd = 0.05, - E = 0x3ff, D = 0x1ff, - ta = 0.5, aa = 0.5, tsm = 0x60, tss = 0x20, asm = 0x30, ass = 0x10, - a0a = 0.5, a0sm = 0x0a, a0ss = 0x05, - ) + scale = 0.0413530725332892, f = 0xcfa, + c = float (0xffefbe) / (1 << 24), + tkp = 1, tkd = 16, akp = 2, akd = 16, + a0kp = 1, a0kd = 0.05, + E = 0x3ff, D = 0x1ff, + ta = 0.5, aa = 0.5, tsm = 0x60, tss = 0x20, asm = 0x30, ass = 0x10, + a0a = 0.5, a0sm = 0x0a, a0ss = 0x05, + ) diff --git a/digital/asserv/tools/inter_asserv.py b/digital/asserv/tools/inter_asserv.py index 44fb8492..1dd47fc3 100644 --- a/digital/asserv/tools/inter_asserv.py +++ b/digital/asserv/tools/inter_asserv.py @@ -40,86 +40,86 @@ class InterAsserv (Inter): """Inter, communicating with the asserv board.""" def __init__ (self, argv): - # Asserv. - if argv[0] == '!': - io = proto.popen_io.PopenIO (argv[1:]) - i = asserv.init.host - else: - io = serial.Serial (argv[0]) - i = asserv.init.target - self.a = Asserv (io, **i) - self.a.async = True - self.a.register_pos (self.pos) - # Inter. - Inter.__init__ (self) - self.tk.createfilehandler (self.a, READABLE, self.read) - self.timeout () + # Asserv. + if argv[0] == '!': + io = proto.popen_io.PopenIO (argv[1:]) + i = asserv.init.host + else: + io = serial.Serial (argv[0]) + i = asserv.init.target + self.a = Asserv (io, **i) + self.a.async = True + self.a.register_pos (self.pos) + # Inter. + Inter.__init__ (self) + self.tk.createfilehandler (self.a, READABLE, self.read) + self.timeout () def createWidgets (self): - Inter.createWidgets (self) - self.ArggButton = Button (self.rightFrame, text = 'Argggg!', - command = self.emergency_stop) - self.ArggButton.pack () - self.actionVar = StringVar () - self.actionVar.set ('goto') - self.actionSetPosButton = Radiobutton (self.rightFrame, - text = 'set pos', value = 'set_pos', - variable = self.actionVar) - self.actionSetPosButton.pack () - self.actionGotoButton = Radiobutton (self.rightFrame, - text = 'goto', value = 'goto', - variable = self.actionVar) - self.actionGotoButton.pack () - self.backwardOkVar = IntVar () - self.backwardOkButton = Checkbutton (self.rightFrame, - text = 'backward ok', variable = self.backwardOkVar) - self.backwardOkButton.pack () + Inter.createWidgets (self) + self.ArggButton = Button (self.rightFrame, text = 'Argggg!', + command = self.emergency_stop) + self.ArggButton.pack () + self.actionVar = StringVar () + self.actionVar.set ('goto') + self.actionSetPosButton = Radiobutton (self.rightFrame, + text = 'set pos', value = 'set_pos', + variable = self.actionVar) + self.actionSetPosButton.pack () + self.actionGotoButton = Radiobutton (self.rightFrame, + text = 'goto', value = 'goto', + variable = self.actionVar) + self.actionGotoButton.pack () + self.backwardOkVar = IntVar () + self.backwardOkButton = Checkbutton (self.rightFrame, + text = 'backward ok', variable = self.backwardOkVar) + self.backwardOkButton.pack () - self.tableview.configure (cursor = 'crosshair') - self.tableview.bind ('<1>', self.button1) - self.tableview.bind ('<3>', self.button3) + self.tableview.configure (cursor = 'crosshair') + self.tableview.bind ('<1>', self.button1) + self.tableview.bind ('<3>', self.button3) def read (self, file, mask): - """Handle asserv events.""" - self.a.proto.read () - self.a.proto.sync () + """Handle asserv events.""" + self.a.proto.read () + self.a.proto.sync () def timeout (self): - self.a.proto.sync () - self.after (100, self.timeout) + self.a.proto.sync () + self.after (100, self.timeout) def pos (self, x, y, a): - self.tableview.robot.pos = (x, y) - self.tableview.robot.angle = float (a) / 180 * math.pi - self.update (self.tableview.robot) - self.update () + self.tableview.robot.pos = (x, y) + self.tableview.robot.angle = float (a) / 180 * math.pi + self.update (self.tableview.robot) + self.update () def button1 (self, ev): - x, y = self.tableview.screen_coord ((ev.x, ev.y)) - action = self.actionVar.get () - if action == 'set_pos': - self.a.set_pos (x, y) - elif action == 'goto': - self.a.goto (x, y, self.backwardOkVar.get ()) - else: - assert 0 + x, y = self.tableview.screen_coord ((ev.x, ev.y)) + action = self.actionVar.get () + if action == 'set_pos': + self.a.set_pos (x, y) + elif action == 'goto': + self.a.goto (x, y, self.backwardOkVar.get ()) + else: + assert 0 def button3 (self, ev): - x, y = self.tableview.screen_coord ((ev.x, ev.y)) - a = math.degrees (math.atan2 (y - self.tableview.robot.pos[1], - x - self.tableview.robot.pos[0])) - action = self.actionVar.get () - if action == 'set_pos': - self.a.set_pos (a = a) - elif action == 'goto': - self.a.goto_angle (a) + x, y = self.tableview.screen_coord ((ev.x, ev.y)) + a = math.degrees (math.atan2 (y - self.tableview.robot.pos[1], + x - self.tableview.robot.pos[0])) + action = self.actionVar.get () + if action == 'set_pos': + self.a.set_pos (a = a) + elif action == 'goto': + self.a.goto_angle (a) def emergency_stop (self): - self.a.free () + self.a.free () if __name__ == '__main__': app = InterAsserv (sys.argv[1:]) try: - app.mainloop () + app.mainloop () finally: - app.a.close () + app.a.close () diff --git a/digital/asserv/tools/step.py b/digital/asserv/tools/step.py index b92311e5..a4538855 100644 --- a/digital/asserv/tools/step.py +++ b/digital/asserv/tools/step.py @@ -8,9 +8,9 @@ import Gnuplot def step (name, offset, kp, ki, kd, plots, **param): if sys.argv[1] == '!': - io = proto.popen_io.PopenIO (sys.argv[2:]) + io = proto.popen_io.PopenIO (sys.argv[2:]) else: - io = serial.Serial (sys.argv[1]) + io = serial.Serial (sys.argv[1]) p = { name + 'kp': kp, name + 'ki': ki, name + 'kd': kd} p.update (param) a = Asserv (io, **p) diff --git a/digital/avr/modules/path/test/test_path.py b/digital/avr/modules/path/test/test_path.py index b7eb63fd..ff9b425c 100644 --- a/digital/avr/modules/path/test/test_path.py +++ b/digital/avr/modules/path/test/test_path.py @@ -31,129 +31,129 @@ import re class Obstacle: def __init__ (self, pos, radius): - self.pos = pos - self.radius = radius + self.pos = pos + self.radius = radius def move (self, pos): - self.pos = pos + self.pos = pos class Area (Drawable): def __init__ (self, onto, border_min, border_max): - Drawable.__init__ (self, onto) - self.border_min = border_min - self.border_max = border_max - self.border = None - self.src = None - self.dst = None - self.obstacles = [ ] - self.path = [ ] + Drawable.__init__ (self, onto) + self.border_min = border_min + self.border_max = border_max + self.border = None + self.src = None + self.dst = None + self.obstacles = [ ] + self.path = [ ] def draw (self): - self.reset () - self.draw_rectangle (self.border_min, self.border_max, fill = 'white') - for o in self.obstacles: - if o.pos is not None: - self.draw_circle (o.pos, o.radius, fill = 'gray25') - if self.src is not None: - self.draw_circle (self.src, 10, fill = 'green') - if self.dst is not None: - self.draw_circle (self.dst, 10, fill = 'red') - if len (self.path) > 1: - fmt = dict (fill = 'blue', arrow = LAST) - self.draw_line (*self.path, **fmt) + self.reset () + self.draw_rectangle (self.border_min, self.border_max, fill = 'white') + for o in self.obstacles: + if o.pos is not None: + self.draw_circle (o.pos, o.radius, fill = 'gray25') + if self.src is not None: + self.draw_circle (self.src, 10, fill = 'green') + if self.dst is not None: + self.draw_circle (self.dst, 10, fill = 'red') + if len (self.path) > 1: + fmt = dict (fill = 'blue', arrow = LAST) + self.draw_line (*self.path, **fmt) def test (self): - self.src = (300, 750) - self.dst = (1200, 750) - self.obstacles.append (Obstacle ((600, 680), 100)) - self.obstacles.append (Obstacle ((900, 820), 100)) + self.src = (300, 750) + self.dst = (1200, 750) + self.obstacles.append (Obstacle ((600, 680), 100)) + self.obstacles.append (Obstacle ((900, 820), 100)) def update (self): - args = [ [ self.border_min[0], self.border_min[1], self.border_max[0], - self.border_max[1] ], self.src, self.dst ] - for o in self.obstacles: - args.append ([o.pos[0], o.pos[1], o.radius]) - args = [ ','.join (str (ai) for ai in a) for a in args ] - args[0:0] = [ './test_path.host' ] - p = Popen (args, stdout = PIPE) - output = p.communicate ()[0] - del p - output = output.split ('\n') - r = re.compile ('^// (\d+), (\d+)$') - self.path = [ ] - for line in output: - m = r.match (line) - if m is not None: - self.path.append (tuple (int (s) for s in m.groups ())) + args = [ [ self.border_min[0], self.border_min[1], self.border_max[0], + self.border_max[1] ], self.src, self.dst ] + for o in self.obstacles: + args.append ([o.pos[0], o.pos[1], o.radius]) + args = [ ','.join (str (ai) for ai in a) for a in args ] + args[0:0] = [ './test_path.host' ] + p = Popen (args, stdout = PIPE) + output = p.communicate ()[0] + del p + output = output.split ('\n') + r = re.compile ('^// (\d+), (\d+)$') + self.path = [ ] + for line in output: + m = r.match (line) + if m is not None: + self.path.append (tuple (int (s) for s in m.groups ())) class AreaView (DrawableCanvas): def __init__ (self, border_min, border_max, master = None): - self.border_min = border_min - self.border_max = border_max - width = border_max[0] - border_min[0] - height = border_max[1] - border_min[0] - DrawableCanvas.__init__ (self, width * 1.1, height * 1.1, -width / 2, - -height / 2, - master, borderwidth = 1, relief = 'sunken', - background = 'white') - self.area = Area (self, border_min, border_max) - self.area.test () - self.area.update () + self.border_min = border_min + self.border_max = border_max + width = border_max[0] - border_min[0] + height = border_max[1] - border_min[0] + DrawableCanvas.__init__ (self, width * 1.1, height * 1.1, -width / 2, + -height / 2, + master, borderwidth = 1, relief = 'sunken', + background = 'white') + self.area = Area (self, border_min, border_max) + self.area.test () + self.area.update () def draw (self): - self.area.draw () + self.area.draw () class TestPath (Frame): def __init__ (self, border_min, border_max, master = None): - Frame.__init__ (self, master) + Frame.__init__ (self, master) self.pack (expand = 1, fill = 'both') self.createWidgets (border_min, border_max) - self.move = None + self.move = None def createWidgets (self, border_min, border_max): - self.rightFrame = Frame (self) - self.rightFrame.pack (side = 'right', fill = 'y') - self.quitButton = Button (self.rightFrame, text = 'Quit', command = self.quit) - self.quitButton.pack (side = 'top', fill = 'x') - self.areaview = AreaView (border_min, border_max, self) - self.areaview.pack (expand = True, fill = 'both') - self.areaview.bind ('<1>', self.click) + self.rightFrame = Frame (self) + self.rightFrame.pack (side = 'right', fill = 'y') + self.quitButton = Button (self.rightFrame, text = 'Quit', command = self.quit) + self.quitButton.pack (side = 'top', fill = 'x') + self.areaview = AreaView (border_min, border_max, self) + self.areaview.pack (expand = True, fill = 'both') + self.areaview.bind ('<1>', self.click) def clear (self): - self.areaview.area.path = [ ] - self.areaview.area.draw () + self.areaview.area.path = [ ] + self.areaview.area.draw () def update (self): - self.areaview.area.update () - self.areaview.area.draw () + self.areaview.area.update () + self.areaview.area.draw () def click (self, ev): - pos = self.areaview.screen_coord ((ev.x, ev.y)) - pos = tuple (int (i) for i in pos) - if self.move is None: - def move_src (pos): - self.areaview.area.src = pos - def move_dst (pos): - self.areaview.area.dst = pos - objs = [ [ self.areaview.area.src, 10, move_src ], - [ self.areaview.area.dst, 10, move_dst ] ] - for o in self.areaview.area.obstacles: - objs.append ([ o.pos, o.radius, o.move ]) - for obj in objs: - dx = obj[0][0] - pos[0] - dy = obj[0][1] - pos[1] - if dx * dx + dy * dy < obj[1] * obj[1]: - self.move = obj[2] - if self.move is not None: - self.move (None) - self.clear () - else: - self.move (pos) - self.update () - self.move = None + pos = self.areaview.screen_coord ((ev.x, ev.y)) + pos = tuple (int (i) for i in pos) + if self.move is None: + def move_src (pos): + self.areaview.area.src = pos + def move_dst (pos): + self.areaview.area.dst = pos + objs = [ [ self.areaview.area.src, 10, move_src ], + [ self.areaview.area.dst, 10, move_dst ] ] + for o in self.areaview.area.obstacles: + objs.append ([ o.pos, o.radius, o.move ]) + for obj in objs: + dx = obj[0][0] - pos[0] + dy = obj[0][1] - pos[1] + if dx * dx + dy * dy < obj[1] * obj[1]: + self.move = obj[2] + if self.move is not None: + self.move (None) + self.clear () + else: + self.move (pos) + self.update () + self.move = None if __name__ == '__main__': app = TestPath ((0, 0), (1500, 1500)) diff --git a/digital/avr/modules/twi/test/test_twi_host.py b/digital/avr/modules/twi/test/test_twi_host.py index 10b3e208..5af0e669 100644 --- a/digital/avr/modules/twi/test/test_twi_host.py +++ b/digital/avr/modules/twi/test/test_twi_host.py @@ -17,8 +17,8 @@ fh = Forked (h.wait) n = Node () def nf (): while True: - time.sleep (1) - n.wait (n.date + 1) + time.sleep (1) + n.wait (n.date + 1) fn = Forked (nf) slave = os.popen ('./test_twi_sl.host', 'w') diff --git a/digital/io/tools/io/init.py b/digital/io/tools/io/init.py index 12c6bb9a..7b8d2b8a 100644 --- a/digital/io/tools/io/init.py +++ b/digital/io/tools/io/init.py @@ -1,6 +1,6 @@ """Default parameters for io.""" host = dict ( - trap = ((1, 255), (1, 255), (1, 255), (1, 255), (1, 255), (1, 255)), - sharp_threshold = ((0x7f, 0x90), (0x7f, 0x90), (0x7f, 0x90), - (0x7f, 0x90), (0x7f, 0x90)), - ) + trap = ((1, 255), (1, 255), (1, 255), (1, 255), (1, 255), (1, 255)), + sharp_threshold = ((0x7f, 0x90), (0x7f, 0x90), (0x7f, 0x90), + (0x7f, 0x90), (0x7f, 0x90)), + ) diff --git a/digital/io/tools/io/io.py b/digital/io/tools/io/io.py index 8b78a8cc..5d2826a1 100644 --- a/digital/io/tools/io/io.py +++ b/digital/io/tools/io/io.py @@ -3,34 +3,34 @@ import proto, time class Io: def __init__ (self, file, time = time.time, **param): - self.proto = proto.Proto (file, time, 0.1) - self.async = False - self.param = param - self.send_param () + self.proto = proto.Proto (file, time, 0.1) + self.async = False + self.param = param + self.send_param () def send_param (self): - p = self.param - for i, t in enumerate (p['trap']): - self.proto.send ('t', 'BBB', i, t[0], t[1]) - for i, t in enumerate (p['sharp_threshold']): - self.proto.send ('h', 'BHH', i, t[0], t[1]) + p = self.param + for i, t in enumerate (p['trap']): + self.proto.send ('t', 'BBB', i, t[0], t[1]) + for i, t in enumerate (p['sharp_threshold']): + self.proto.send ('h', 'BHH', i, t[0], t[1]) def write_eeprom (self): - self.proto.send ('p', 'BB', ord ('E'), ord ('s')) - time.sleep (1) - self.proto.wait (lambda: True) + self.proto.send ('p', 'BB', ord ('E'), ord ('s')) + time.sleep (1) + self.proto.wait (lambda: True) def reset (self): - self.proto.send ('w') - self.proto.send ('w', 'H', 0) - self.proto.send ('z') - self.proto.send ('z') + self.proto.send ('w') + self.proto.send ('w', 'H', 0) + self.proto.send ('z') + self.proto.send ('z') def close (self): - self.reset () - self.wait (lambda: True) - self.proto.file.close () + self.reset () + self.wait (lambda: True) + self.proto.file.close () def fileno (self): - return self.proto.fileno () + return self.proto.fileno () diff --git a/digital/io/tools/test_simu.py b/digital/io/tools/test_simu.py index 7f208911..1d7175a7 100644 --- a/digital/io/tools/test_simu.py +++ b/digital/io/tools/test_simu.py @@ -18,58 +18,58 @@ class TestSimu (InterNode): """Inter, with simulated programs.""" robot_start_pos = ((200, 2100 - 70, -90), - (3000 - 200, 2100 - 70, -90)) + (3000 - 200, 2100 - 70, -90)) def __init__ (self, asserv_cmd, io_cmd): - # Hub. - self.hub = mex.hub.Hub (min_clients = 2) - self.forked_hub = utils.forked.Forked (self.hub.wait) - # InterNode. - InterNode.__init__ (self) - def time (): - return self.node.date / self.TICK - # Asserv. - self.asserv = Asserv (PopenIO (asserv_cmd), time, **asserv.init.host) - self.asserv.async = True - self.tk.createfilehandler (self.asserv, READABLE, self.asserv_read) - # Io. - self.io = Io (PopenIO (io_cmd), time, **io.init.host) - self.io.async = True - self.tk.createfilehandler (self.io, READABLE, self.io_read) - # Color switch. - self.change_color () - self.colorVar.trace_variable ('w', self.change_color) + # Hub. + self.hub = mex.hub.Hub (min_clients = 2) + self.forked_hub = utils.forked.Forked (self.hub.wait) + # InterNode. + InterNode.__init__ (self) + def time (): + return self.node.date / self.TICK + # Asserv. + self.asserv = Asserv (PopenIO (asserv_cmd), time, **asserv.init.host) + self.asserv.async = True + self.tk.createfilehandler (self.asserv, READABLE, self.asserv_read) + # Io. + self.io = Io (PopenIO (io_cmd), time, **io.init.host) + self.io.async = True + self.tk.createfilehandler (self.io, READABLE, self.io_read) + # Color switch. + self.change_color () + self.colorVar.trace_variable ('w', self.change_color) def close (self): - self.forked_hub.kill () - import time - time.sleep (1) - self.asserv.close () - self.io.close () + self.forked_hub.kill () + import time + time.sleep (1) + self.asserv.close () + self.io.close () def asserv_read (self, file, mask): - self.asserv.proto.read () - self.asserv.proto.sync () + self.asserv.proto.read () + self.asserv.proto.sync () def io_read (self, file, mask): - self.io.proto.read () - self.io.proto.sync () + self.io.proto.read () + self.io.proto.sync () def step (self): - """Overide step to handle retransmissions, could be made cleaner using - simulated time.""" - InterNode.step (self) - self.asserv.proto.sync () - self.io.proto.sync () + """Overide step to handle retransmissions, could be made cleaner using + simulated time.""" + InterNode.step (self) + self.asserv.proto.sync () + self.io.proto.sync () def change_color (self, *dummy): - i = self.colorVar.get () - self.asserv.set_simu_pos (*self.robot_start_pos[i]); + i = self.colorVar.get () + self.asserv.set_simu_pos (*self.robot_start_pos[i]); if __name__ == '__main__': app = TestSimu (('../../asserv/src/asserv/asserv.host', '-m', 'giboulee'), - ('../src/io.host')) + ('../src/io.host')) try: - app.mainloop () + app.mainloop () finally: - app.close () + app.close () diff --git a/host/inter/dist_sensor.py b/host/inter/dist_sensor.py index 54c61e6d..b1bd0d0d 100644 --- a/host/inter/dist_sensor.py +++ b/host/inter/dist_sensor.py @@ -32,51 +32,51 @@ class DistSensor (Drawable): """A distance sensor.""" def __init__ (self, onto, pos, angle, range): - Drawable.__init__ (self, onto) - self.pos = pos - self.angle = angle - self.range = range - self.target = (pos[0] + cos (angle) * range, - pos[1] + sin (angle) * range) - self.obstacles = [ ] - self.distance = None - self.hide = False + Drawable.__init__ (self, onto) + self.pos = pos + self.angle = angle + self.range = range + self.target = (pos[0] + cos (angle) * range, + pos[1] + sin (angle) * range) + self.obstacles = [ ] + self.distance = None + self.hide = False def compute (self): - # Could do better by trans_applying until on the same drawable as the - # obstacle. - pos = self.onto.trans_apply (self.pos) - target = self.onto.trans_apply (self.target) - n = ((target[0] - pos[0]) / self.range, - (target[1] - pos[1]) / self.range) - self.distance = None - for o in self.obstacles: - # Does the line intersect with the obstacle? - ao = (o.pos[0] - pos[0], o.pos[1] - pos[1]) - doc = abs (ao[0] * -n[1] + ao[1] * n[0]) - if doc < o.radius: - # Does the segment intersect? - m = ao[0] * n[0] + ao[1] * n[1] - f = sqrt (o.radius ** 2 - doc ** 2) - if m - f > 0 and m - f < self.range: - d = m - f - if self.distance is None or self.distance > d: - self.distance = d - elif m + f > 0 and m + f < self.range: - d = m + f - if self.distance is None or self.distance > d: - self.distance = d - return self.distance + # Could do better by trans_applying until on the same drawable as the + # obstacle. + pos = self.onto.trans_apply (self.pos) + target = self.onto.trans_apply (self.target) + n = ((target[0] - pos[0]) / self.range, + (target[1] - pos[1]) / self.range) + self.distance = None + for o in self.obstacles: + # Does the line intersect with the obstacle? + ao = (o.pos[0] - pos[0], o.pos[1] - pos[1]) + doc = abs (ao[0] * -n[1] + ao[1] * n[0]) + if doc < o.radius: + # Does the segment intersect? + m = ao[0] * n[0] + ao[1] * n[1] + f = sqrt (o.radius ** 2 - doc ** 2) + if m - f > 0 and m - f < self.range: + d = m - f + if self.distance is None or self.distance > d: + self.distance = d + elif m + f > 0 and m + f < self.range: + d = m + f + if self.distance is None or self.distance > d: + self.distance = d + return self.distance def draw (self): - self.compute () - self.reset () - if self.hide: - return - if self.distance is None: - self.draw_line (self.pos, self.target, fill = 'blue', arrow = LAST) - else: - inter = (self.pos[0] + cos (self.angle) * self.distance, - self.pos[1] + sin (self.angle) * self.distance) - self.draw_line (self.pos, inter, fill = 'red', arrow = LAST) - self.draw_line (inter, self.target, fill = 'blue', arrow = LAST) + self.compute () + self.reset () + if self.hide: + return + if self.distance is None: + self.draw_line (self.pos, self.target, fill = 'blue', arrow = LAST) + else: + inter = (self.pos[0] + cos (self.angle) * self.distance, + self.pos[1] + sin (self.angle) * self.distance) + self.draw_line (self.pos, inter, fill = 'red', arrow = LAST) + self.draw_line (inter, self.target, fill = 'blue', arrow = LAST) diff --git a/host/inter/drawable.py b/host/inter/drawable.py index 26628f40..81331548 100644 --- a/host/inter/drawable.py +++ b/host/inter/drawable.py @@ -32,78 +32,78 @@ class Drawable: """Define a drawable area with embedded transformations.""" def __init__ (self, onto): - """Initialise the drawable.""" - self.onto = onto - self.trans_matrix = trans_matrix.TransMatrix () - self.items = [ ] - self.trans_apply = self.trans_matrix.apply - self.trans_apply_angle = self.trans_matrix.apply_angle - self.trans_apply_distance = self.trans_matrix.apply_distance - self.trans_rotate = self.trans_matrix.rotate - self.trans_translate = self.trans_matrix.translate - self.trans_scale = self.trans_matrix.scale + """Initialise the drawable.""" + self.onto = onto + self.trans_matrix = trans_matrix.TransMatrix () + self.items = [ ] + self.trans_apply = self.trans_matrix.apply + self.trans_apply_angle = self.trans_matrix.apply_angle + self.trans_apply_distance = self.trans_matrix.apply_distance + self.trans_rotate = self.trans_matrix.rotate + self.trans_translate = self.trans_matrix.translate + self.trans_scale = self.trans_matrix.scale def __draw_rectangle (self, p1, p2, **kw): - if 'outline' not in kw: - kw = kw.copy () - kw['outline'] = 'black' - p = self.trans_apply (p1, (p2[0], p1[1]), p2, (p1[0], p2[1])) - return self.onto.__draw_polygon (*p, **kw) + if 'outline' not in kw: + kw = kw.copy () + kw['outline'] = 'black' + p = self.trans_apply (p1, (p2[0], p1[1]), p2, (p1[0], p2[1])) + return self.onto.__draw_polygon (*p, **kw) def __draw_line (self, *p, **kw): - p = self.trans_apply (*p) - return self.onto.__draw_line (*p, **kw) + p = self.trans_apply (*p) + return self.onto.__draw_line (*p, **kw) def __draw_polygon (self, *p, **kw): - p = self.trans_apply (*p) - return self.onto.__draw_polygon (*p, **kw) + p = self.trans_apply (*p) + return self.onto.__draw_polygon (*p, **kw) def __draw_circle (self, p, r, **kw): - p = self.trans_apply (p) - r = self.trans_apply_distance (r) - return self.onto.__draw_circle (p, r, **kw) + p = self.trans_apply (p) + r = self.trans_apply_distance (r) + return self.onto.__draw_circle (p, r, **kw) def __draw_arc (self, p, r, **kw): - p = self.trans_apply (p) - r = self.trans_apply_distance (r) - if 'start' in kw: - kw = kw.copy () - kw['start'] = self.trans_apply_angle (kw['start']) - import math - return self.onto.__draw_arc (p, r, **kw) + p = self.trans_apply (p) + r = self.trans_apply_distance (r) + if 'start' in kw: + kw = kw.copy () + kw['start'] = self.trans_apply_angle (kw['start']) + import math + return self.onto.__draw_arc (p, r, **kw) def draw_rectangle (self, *p, **kw): - """Draw a rectangle.""" - self.items.append (self.__draw_rectangle (*p, **kw)) + """Draw a rectangle.""" + self.items.append (self.__draw_rectangle (*p, **kw)) def draw_line (self, *p, **kw): - """Draw a line.""" - self.items.append (self.__draw_line (*p, **kw)) + """Draw a line.""" + self.items.append (self.__draw_line (*p, **kw)) def draw_polygon (self, *p, **kw): - """Draw a line.""" - self.items.append (self.__draw_polygon (*p, **kw)) + """Draw a line.""" + self.items.append (self.__draw_polygon (*p, **kw)) def draw_circle (self, p, r, **kw): - """Draw a circle of the given radius centered on p.""" - self.items.append (self.__draw_circle (p, r, **kw)) + """Draw a circle of the given radius centered on p.""" + self.items.append (self.__draw_circle (p, r, **kw)) def draw_arc (self, p, r, **kw): - """Draw a arc of the given radius centered on p.""" - self.items.append (self.__draw_arc (p, r, **kw)) + """Draw a arc of the given radius centered on p.""" + self.items.append (self.__draw_arc (p, r, **kw)) def trans_reset (self): - """Reset transformations.""" - self.trans_matrix.identity () + """Reset transformations.""" + self.trans_matrix.identity () def reset (self): - self.__delete (*self.items) - self.items = [ ] - self.trans_reset () + self.__delete (*self.items) + self.items = [ ] + self.trans_reset () def __delete (self, *list): - """Delete a list of items.""" - self.onto.__delete (*list) + """Delete a list of items.""" + self.onto.__delete (*list) class DrawableCanvas(Tkinter.Canvas): @@ -111,62 +111,62 @@ class DrawableCanvas(Tkinter.Canvas): the draw method.""" def __init__ (self, width, height, xorigin, yorigin, master = None, **kw): - """Initialise a DrawableCanvas. The width and height parameters - define the requested drawable area virtual size. The xorigin and - yorigin parameters define origin of the virtual coordinates relative - to the drawable center.""" - Tkinter.Canvas.__init__ (self, master, **kw) - self.__width = width - self.__height = height - self.__xorigin = xorigin - self.__yorigin = yorigin - self.bind ('', self.__resize) + """Initialise a DrawableCanvas. The width and height parameters + define the requested drawable area virtual size. The xorigin and + yorigin parameters define origin of the virtual coordinates relative + to the drawable center.""" + Tkinter.Canvas.__init__ (self, master, **kw) + self.__width = width + self.__height = height + self.__xorigin = xorigin + self.__yorigin = yorigin + self.bind ('', self.__resize) def __resize (self, ev): - # Compute new scale. - w, h = float (ev.width), float (ev.height) - self.__scale = min (w / self.__width, h / self.__height) - self.__xoffset = w / 2 + self.__xorigin * self.__scale - self.__yoffset = h / 2 - self.__yorigin * self.__scale - # Redraw. - self.draw () + # Compute new scale. + w, h = float (ev.width), float (ev.height) + self.__scale = min (w / self.__width, h / self.__height) + self.__xoffset = w / 2 + self.__xorigin * self.__scale + self.__yoffset = h / 2 - self.__yorigin * self.__scale + # Redraw. + self.draw () def _Drawable__draw_line (self, *p, **kw): - p = self.__coord (*p) - return self.create_line (*p, **kw) + p = self.__coord (*p) + return self.create_line (*p, **kw) def _Drawable__draw_polygon (self, *p, **kw): - p = self.__coord (*p) - return self.create_polygon (*p, **kw) + p = self.__coord (*p) + return self.create_polygon (*p, **kw) def _Drawable__draw_circle (self, p, r, **kw): - p, = self.__coord (p) - r = r * self.__scale - p1 = (p[0] - r, p[1] - r) - p2 = (p[0] + r, p[1] + r) - return self.create_oval (p1, p2, **kw) + p, = self.__coord (p) + r = r * self.__scale + p1 = (p[0] - r, p[1] - r) + p2 = (p[0] + r, p[1] + r) + return self.create_oval (p1, p2, **kw) def _Drawable__draw_arc (self, p, r, **kw): - p, = self.__coord (p) - r = r * self.__scale - p1 = (p[0] - r, p[1] - r) - p2 = (p[0] + r, p[1] + r) - for k in ('start', 'extent'): - if k in kw: - kw = kw.copy () - kw[k] = degrees (kw[k]) - return self.create_arc (p1, p2, **kw) + p, = self.__coord (p) + r = r * self.__scale + p1 = (p[0] - r, p[1] - r) + p2 = (p[0] + r, p[1] + r) + for k in ('start', 'extent'): + if k in kw: + kw = kw.copy () + kw[k] = degrees (kw[k]) + return self.create_arc (p1, p2, **kw) def _Drawable__delete (self, *list): - self.delete (*list) + self.delete (*list) def __coord (self, *args): - return [ (i[0] * self.__scale + self.__xoffset, - -i[1] * self.__scale + self.__yoffset) for i in args ] + return [ (i[0] * self.__scale + self.__xoffset, + -i[1] * self.__scale + self.__yoffset) for i in args ] def screen_coord (self, screen): - """Return drawable coordinates corresponding to the given screen - coordinates.""" - return ((self.canvasx (screen[0]) - self.__xoffset) / self.__scale, - -(self.canvasy (screen[1]) - self.__yoffset) / self.__scale) + """Return drawable coordinates corresponding to the given screen + coordinates.""" + return ((self.canvasx (screen[0]) - self.__xoffset) / self.__scale, + -(self.canvasy (screen[1]) - self.__yoffset) / self.__scale) diff --git a/host/inter/inter.py b/host/inter/inter.py index cde1bd73..4c19e5c8 100644 --- a/host/inter/inter.py +++ b/host/inter/inter.py @@ -31,105 +31,105 @@ class Robot (Drawable): """The robot.""" def __init__ (self, onto): - Drawable.__init__ (self, onto) - self.drawn = [ ] + Drawable.__init__ (self, onto) + self.drawn = [ ] def draw (self): - self.reset () - self.trans_rotate (self.angle) - self.trans_translate (self.pos) - self.draw_polygon ((115, 30), (170, 85), (150, 127), (130, 145), - (-25, 200), (-70, 200), (-70, -200), (-25, -200), - (130, -145), (150, -127), (170, -85), (115, -30)) - axes_fill = '#404040' - self.draw_line ((-70, 0), (170, 0), fill = axes_fill, arrow = LAST) - f = 142 + 2 * 31.5 - 13.5 - wr = 63 / 2 - self.draw_line ((0, +f / 2), (0, -f / 2), fill = axes_fill) - self.draw_line ((-wr, f / 2), (+wr, f / 2), fill = axes_fill) - self.draw_line ((-wr, -f / 2), (+wr, -f / 2), fill = axes_fill) - for i in self.drawn: - i.draw () + self.reset () + self.trans_rotate (self.angle) + self.trans_translate (self.pos) + self.draw_polygon ((115, 30), (170, 85), (150, 127), (130, 145), + (-25, 200), (-70, 200), (-70, -200), (-25, -200), + (130, -145), (150, -127), (170, -85), (115, -30)) + axes_fill = '#404040' + self.draw_line ((-70, 0), (170, 0), fill = axes_fill, arrow = LAST) + f = 142 + 2 * 31.5 - 13.5 + wr = 63 / 2 + self.draw_line ((0, +f / 2), (0, -f / 2), fill = axes_fill) + self.draw_line ((-wr, f / 2), (+wr, f / 2), fill = axes_fill) + self.draw_line ((-wr, -f / 2), (+wr, -f / 2), fill = axes_fill) + for i in self.drawn: + i.draw () class Obstacle (Drawable): """An obstacle.""" def __init__ (self, onto, pos, radius): - Drawable.__init__ (self, onto) - self.pos = pos - self.radius = radius + Drawable.__init__ (self, onto) + self.pos = pos + self.radius = radius def draw (self): - self.reset () - self.trans_translate (self.pos) - self.draw_circle ((0, 0), self.radius, fill = '#31aa23') - self.draw_circle ((0, 0), self.radius + 250, outlinestipple = 'gray25') + self.reset () + self.trans_translate (self.pos) + self.draw_circle ((0, 0), self.radius, fill = '#31aa23') + self.draw_circle ((0, 0), self.radius + 250, outlinestipple = 'gray25') class Table (Drawable): """The table and its elements.""" def draw (self): - # Redraw. - self.reset () - # Table. - self.draw_rectangle ((-22, -22 - 80), (3000 / 2, 2100 + 22), fill = '#ff1f1f') - self.draw_rectangle ((3000 / 2, -22 - 80), (3000 + 22, 2100 + 22), fill = '#201fff') - self.draw_rectangle ((0, 0), (3000, 2100), fill = '#a49d8b') - self.draw_rectangle ((0, -22 - 80), (3000, -22), fill = '#a49d8b') - self.draw_rectangle ((0, 2100 - 500), (500, 2100), fill = '#201fff') - self.draw_rectangle ((3000 - 500, 2100 - 500), (3000, 2100), fill = '#ff1f1f') - self.draw_line ((3000 / 2, -22 - 80), (3000 / 2, 2100 + 22)) - # Axes. - self.draw_line ((0, 200), (0, 0), (200, 0), arrow = BOTH) - # Beacons and baskets. - self.draw_rectangle ((-22, 2100), (-22 - 80, 2100 + 80), fill = '#5b5b5d') - self.draw_rectangle ((-22, 1050 - 40), (-22 - 80, 1050 + 40), fill = '#5b5b5d') - self.draw_rectangle ((-22, 500), (-22 - 80, 500 + 80), fill = '#5b5b5d') - self.draw_rectangle ((-22, -80), (-22 - 80, 0), fill = '#5b5b5d') - self.draw_rectangle ((-22, 0), (-22 - 80, 500), fill = '#5b5b5d') - self.draw_rectangle ((-22 - 80 - 250, 0), (-22 - 80, 500), fill = '#6d6dad', stipple = 'gray75') - self.draw_rectangle ((3000 + 22, 2100), (3000 + 22 + 80, 2100 + 80), fill = '#5b5b5d') - self.draw_rectangle ((3000 + 22, 1050 - 40), (3000 + 22 + 80, 1050 + 40), fill = '#5b5b5d') - self.draw_rectangle ((3000 + 22, 500), (3000 + 22 + 80, 500 + 80), fill = '#5b5b5d') - self.draw_rectangle ((3000 + 22, -80), (3000 + 22 + 80, 0), fill = '#5b5b5d') - self.draw_rectangle ((3000 + 22, 0), (3000 + 22 + 80, 500), fill = '#5b5b5d') - self.draw_rectangle ((3000 + 22 + 80 + 250, 0), (3000 + 22 + 80, 500), fill = '#6d6dad', stipple = 'gray75') - # Vertical dispensers. - self.draw_rectangle ((-22, 2100 - 750 - 85 / 2), (0, 2100 - 750 + 85 / 2), fill = '#5b5b5b') - self.draw_circle ((40, 2100 - 750), 40) - self.draw_rectangle ((700 - 85 / 2, 2100), (700 + 85 / 2, 2100 + 22), fill = '#5b5b5b') - self.draw_circle ((700, 2100 - 40), 40) - self.draw_rectangle ((3000 + 22, 2100 - 750 - 85 / 2), (3000, 2100 - 750 + 85 / 2), fill = '#5b5b5b') - self.draw_circle ((3000 - 40, 2100 - 750), 40) - self.draw_rectangle ((3000 - 700 + 85 / 2, 2100), (3000 - 700 - 85 / 2, 2100 + 22), fill = '#5b5b5b') - self.draw_circle ((3000 - 700, 2100 - 40), 40) - # Horizontal dispenser. - self.draw_rectangle ((3000 / 2 - 924 / 2, 2100 + 22), (3000 / 2 + 924 / 2, 2100 + 22 + 80 + 22), fill = '#5b5b5b') - self.draw_rectangle ((3000 / 2 - 924 / 2 + 22, 2100 + 22), (3000 / 2 + 924 / 2 - 22, 2100 + 22 + 80), fill = '#5b5b5b') - self.draw_rectangle ((3000 / 2 - 880 / 2 - 35 - 60, 2100), (3000 / 2 - 880 / 2 - 35, 2100 + 44), fill = '#5b5b5b') - self.draw_rectangle ((3000 / 2 + 880 / 2 + 35 + 60, 2100), (3000 / 2 + 880 / 2 + 35, 2100 + 44), fill = '#5b5b5b') - # Balls. - balls = [ (800, 200, 'rb'), (800, 400, 'RB'), (800, 600, 'ww'), - (1300, 200, 'rb'), (1300, 400, 'rb'), (1300, 600, 'ww'), - (520, 800, 'WW'), (700, 40, 'RB'), (40, 750, 'WW'), - (450, 1120, 'ww'), (750, 1070, 'ww'), (1050, 1020, 'ww'), - (1500 - 72 / 2, -22 - 80 / 2, 'BR'), - (1500 - 72 / 2 - 1 * 73, -22 - 80 / 2, 'RB'), - (1500 - 72 / 2 - 2 * 73, -22 - 80 / 2, 'BR'), - (1500 - 72 / 2 - 3 * 73, -22 - 80 / 2, 'RB'), - (1500 - 72 / 2 - 4 * 73, -22 - 80 / 2, 'BR'), - (1500 - 72 / 2 - 5 * 73, -22 - 80 / 2, 'RB'), - (1500, 1000, 'W'), - ] - balls_config = { 'r': { 'outline': '#bf4141' }, 'R': { 'fill': '#bf4141' }, - 'b': { 'outline': '#4241bf' }, 'B': { 'fill': '#4241bf' }, - 'w': { 'outline': '#bfbfbf' }, 'W': { 'fill': '#bfbfbf' } } - for b in balls: - self.draw_circle ((3000 - b[0], 2100 - b[1]), 72 / 2, - **balls_config[b[2][0]]) - if len (b[2]) > 1: - self.draw_circle ((b[0], 2100 - b[1]), 72 / 2, - **balls_config[b[2][1]]) + # Redraw. + self.reset () + # Table. + self.draw_rectangle ((-22, -22 - 80), (3000 / 2, 2100 + 22), fill = '#ff1f1f') + self.draw_rectangle ((3000 / 2, -22 - 80), (3000 + 22, 2100 + 22), fill = '#201fff') + self.draw_rectangle ((0, 0), (3000, 2100), fill = '#a49d8b') + self.draw_rectangle ((0, -22 - 80), (3000, -22), fill = '#a49d8b') + self.draw_rectangle ((0, 2100 - 500), (500, 2100), fill = '#201fff') + self.draw_rectangle ((3000 - 500, 2100 - 500), (3000, 2100), fill = '#ff1f1f') + self.draw_line ((3000 / 2, -22 - 80), (3000 / 2, 2100 + 22)) + # Axes. + self.draw_line ((0, 200), (0, 0), (200, 0), arrow = BOTH) + # Beacons and baskets. + self.draw_rectangle ((-22, 2100), (-22 - 80, 2100 + 80), fill = '#5b5b5d') + self.draw_rectangle ((-22, 1050 - 40), (-22 - 80, 1050 + 40), fill = '#5b5b5d') + self.draw_rectangle ((-22, 500), (-22 - 80, 500 + 80), fill = '#5b5b5d') + self.draw_rectangle ((-22, -80), (-22 - 80, 0), fill = '#5b5b5d') + self.draw_rectangle ((-22, 0), (-22 - 80, 500), fill = '#5b5b5d') + self.draw_rectangle ((-22 - 80 - 250, 0), (-22 - 80, 500), fill = '#6d6dad', stipple = 'gray75') + self.draw_rectangle ((3000 + 22, 2100), (3000 + 22 + 80, 2100 + 80), fill = '#5b5b5d') + self.draw_rectangle ((3000 + 22, 1050 - 40), (3000 + 22 + 80, 1050 + 40), fill = '#5b5b5d') + self.draw_rectangle ((3000 + 22, 500), (3000 + 22 + 80, 500 + 80), fill = '#5b5b5d') + self.draw_rectangle ((3000 + 22, -80), (3000 + 22 + 80, 0), fill = '#5b5b5d') + self.draw_rectangle ((3000 + 22, 0), (3000 + 22 + 80, 500), fill = '#5b5b5d') + self.draw_rectangle ((3000 + 22 + 80 + 250, 0), (3000 + 22 + 80, 500), fill = '#6d6dad', stipple = 'gray75') + # Vertical dispensers. + self.draw_rectangle ((-22, 2100 - 750 - 85 / 2), (0, 2100 - 750 + 85 / 2), fill = '#5b5b5b') + self.draw_circle ((40, 2100 - 750), 40) + self.draw_rectangle ((700 - 85 / 2, 2100), (700 + 85 / 2, 2100 + 22), fill = '#5b5b5b') + self.draw_circle ((700, 2100 - 40), 40) + self.draw_rectangle ((3000 + 22, 2100 - 750 - 85 / 2), (3000, 2100 - 750 + 85 / 2), fill = '#5b5b5b') + self.draw_circle ((3000 - 40, 2100 - 750), 40) + self.draw_rectangle ((3000 - 700 + 85 / 2, 2100), (3000 - 700 - 85 / 2, 2100 + 22), fill = '#5b5b5b') + self.draw_circle ((3000 - 700, 2100 - 40), 40) + # Horizontal dispenser. + self.draw_rectangle ((3000 / 2 - 924 / 2, 2100 + 22), (3000 / 2 + 924 / 2, 2100 + 22 + 80 + 22), fill = '#5b5b5b') + self.draw_rectangle ((3000 / 2 - 924 / 2 + 22, 2100 + 22), (3000 / 2 + 924 / 2 - 22, 2100 + 22 + 80), fill = '#5b5b5b') + self.draw_rectangle ((3000 / 2 - 880 / 2 - 35 - 60, 2100), (3000 / 2 - 880 / 2 - 35, 2100 + 44), fill = '#5b5b5b') + self.draw_rectangle ((3000 / 2 + 880 / 2 + 35 + 60, 2100), (3000 / 2 + 880 / 2 + 35, 2100 + 44), fill = '#5b5b5b') + # Balls. + balls = [ (800, 200, 'rb'), (800, 400, 'RB'), (800, 600, 'ww'), + (1300, 200, 'rb'), (1300, 400, 'rb'), (1300, 600, 'ww'), + (520, 800, 'WW'), (700, 40, 'RB'), (40, 750, 'WW'), + (450, 1120, 'ww'), (750, 1070, 'ww'), (1050, 1020, 'ww'), + (1500 - 72 / 2, -22 - 80 / 2, 'BR'), + (1500 - 72 / 2 - 1 * 73, -22 - 80 / 2, 'RB'), + (1500 - 72 / 2 - 2 * 73, -22 - 80 / 2, 'BR'), + (1500 - 72 / 2 - 3 * 73, -22 - 80 / 2, 'RB'), + (1500 - 72 / 2 - 4 * 73, -22 - 80 / 2, 'BR'), + (1500 - 72 / 2 - 5 * 73, -22 - 80 / 2, 'RB'), + (1500, 1000, 'W'), + ] + balls_config = { 'r': { 'outline': '#bf4141' }, 'R': { 'fill': '#bf4141' }, + 'b': { 'outline': '#4241bf' }, 'B': { 'fill': '#4241bf' }, + 'w': { 'outline': '#bfbfbf' }, 'W': { 'fill': '#bfbfbf' } } + for b in balls: + self.draw_circle ((3000 - b[0], 2100 - b[1]), 72 / 2, + **balls_config[b[2][0]]) + if len (b[2]) > 1: + self.draw_circle ((b[0], 2100 - b[1]), 72 / 2, + **balls_config[b[2][1]]) class TableView (DrawableCanvas): """This class handle the view of the table and every items inside it.""" @@ -140,102 +140,102 @@ class TableView (DrawableCanvas): YORIGIN = -2100 / 2 def __init__ (self, master = None): - DrawableCanvas.__init__ (self, self.WIDTH, self.HEIGHT, self.XORIGIN, - self.YORIGIN, - master, borderwidth = 1, relief = 'sunken', - background = 'white') - self.table = Table (self) - self.robot = Robot (self.table) - self.robot.angle = 0 - self.robot.pos = (0, 0) - self.drawn = [ ] + DrawableCanvas.__init__ (self, self.WIDTH, self.HEIGHT, self.XORIGIN, + self.YORIGIN, + master, borderwidth = 1, relief = 'sunken', + background = 'white') + self.table = Table (self) + self.robot = Robot (self.table) + self.robot.angle = 0 + self.robot.pos = (0, 0) + self.drawn = [ ] def draw (self): - self.table.draw () - for i in self.drawn: - i.draw () - self.robot.draw () + self.table.draw () + for i in self.drawn: + i.draw () + self.robot.draw () class Arm (Drawable): """The robot arm.""" def draw (self): - self.reset () - self.draw_arc ((0, 0), 0.45, start = 7 * pi / 12, extent = 10 * pi / 12, - style = 'arc', outline = '#808080') - self.draw_arc ((0, 0), 0.45, start = -5 * pi / 12, extent = 10 * pi / 12, - style = 'arc', outline = '#808080') - self.draw_arc ((0, 0), 0.25, start = -7 * pi / 12, extent = 14 * pi / 12, - style = 'arc', outline = '#808080') - self.trans_scale (0.4) - self.trans_rotate (-self.angle) - self.draw_line ((0, 0), (0, 1)) - self.draw_line ((0, 1), (0.3, 1), arrow = LAST, fill = '#808080') - self.draw_line ((0, 0), (cos (pi / 6), -sin (pi / 6))) - self.draw_line ((0, 0), (-cos (pi / 6), -sin (pi / 6))) + self.reset () + self.draw_arc ((0, 0), 0.45, start = 7 * pi / 12, extent = 10 * pi / 12, + style = 'arc', outline = '#808080') + self.draw_arc ((0, 0), 0.45, start = -5 * pi / 12, extent = 10 * pi / 12, + style = 'arc', outline = '#808080') + self.draw_arc ((0, 0), 0.25, start = -7 * pi / 12, extent = 14 * pi / 12, + style = 'arc', outline = '#808080') + self.trans_scale (0.4) + self.trans_rotate (-self.angle) + self.draw_line ((0, 0), (0, 1)) + self.draw_line ((0, 1), (0.3, 1), arrow = LAST, fill = '#808080') + self.draw_line ((0, 0), (cos (pi / 6), -sin (pi / 6))) + self.draw_line ((0, 0), (-cos (pi / 6), -sin (pi / 6))) class Servo: """Servo motor.""" def __init__ (self, coord, l, start, extent): - self.coord = coord - self.l = l - self.start = start - self.extent = extent - self.pos = 0 + self.coord = coord + self.l = l + self.start = start + self.extent = extent + self.pos = 0 def draw (self, d): - d.draw_arc (self.coord, self.l, start = self.start, - extent = self.extent, style = 'arc', outline = '#808080') - a = self.start + self.pos * self.extent - d.draw_line (self.coord, (self.coord[0] + self.l * cos (a), - self.coord[1] + self.l * sin (a))) + d.draw_arc (self.coord, self.l, start = self.start, + extent = self.extent, style = 'arc', outline = '#808080') + a = self.start + self.pos * self.extent + d.draw_line (self.coord, (self.coord[0] + self.l * cos (a), + self.coord[1] + self.l * sin (a))) class Rear (Drawable): """Rear actuators.""" def __init__ (self, onto): - Drawable.__init__ (self, onto) - self.traps = [ - Servo ((-2.5, -1), 0.8, 0, pi/2), - Servo ((-1.5, -0.9), 0.8, 0, pi/2), - Servo ((-0.5, -0.8), 0.8, 0, pi/2), - Servo ((0.5, -0.8), 0.8, pi, -pi/2), - Servo ((1.5, -0.9), 0.8, pi, -pi/2), - Servo ((-2.5, 1.3), 0.8, -pi/6, pi/3), - ] + Drawable.__init__ (self, onto) + self.traps = [ + Servo ((-2.5, -1), 0.8, 0, pi/2), + Servo ((-1.5, -0.9), 0.8, 0, pi/2), + Servo ((-0.5, -0.8), 0.8, 0, pi/2), + Servo ((0.5, -0.8), 0.8, pi, -pi/2), + Servo ((1.5, -0.9), 0.8, pi, -pi/2), + Servo ((-2.5, 1.3), 0.8, -pi/6, pi/3), + ] def draw (self): - self.reset () - self.trans_scale (0.9/5) - for i in self.traps: - i.draw (self) - self.draw_line ((-0.5, 1.5), (-0.5, 0.5), (-2.5, 0.2), - fill = '#808080') - self.draw_line ((-2.5, -1.2), (-2.5, -2.3), (2.5, -2.3), (2.5, 0.2), - (0.5, 0.5), (0.5, 1.5), fill = '#808080') - for i in (-1.5, -0.5, 0.5, 1.5): - self.draw_line ((i, -2.3), (i, -2), fill = '#808080') + self.reset () + self.trans_scale (0.9/5) + for i in self.traps: + i.draw (self) + self.draw_line ((-0.5, 1.5), (-0.5, 0.5), (-2.5, 0.2), + fill = '#808080') + self.draw_line ((-2.5, -1.2), (-2.5, -2.3), (2.5, -2.3), (2.5, 0.2), + (0.5, 0.5), (0.5, 1.5), fill = '#808080') + for i in (-1.5, -0.5, 0.5, 1.5): + self.draw_line ((i, -2.3), (i, -2), fill = '#808080') class ActuatorView (DrawableCanvas): """This class handle the view of the actuators inside the robot.""" def __init__ (self, master = None): - DrawableCanvas.__init__ (self, 1, 2, 0, 0, master, - borderwidth = 1, relief = 'sunken', background = 'white') - self.configure (width = 120, height = 240) - self.arm_drawable = Drawable (self) - self.arm_drawable.trans_translate ((0, 0.5)) - self.arm = Arm (self.arm_drawable) - self.arm.angle = 0 - self.rear_drawable = Drawable (self) - self.rear_drawable.trans_translate ((0, -0.5)) - self.rear = Rear (self.rear_drawable) + DrawableCanvas.__init__ (self, 1, 2, 0, 0, master, + borderwidth = 1, relief = 'sunken', background = 'white') + self.configure (width = 120, height = 240) + self.arm_drawable = Drawable (self) + self.arm_drawable.trans_translate ((0, 0.5)) + self.arm = Arm (self.arm_drawable) + self.arm.angle = 0 + self.rear_drawable = Drawable (self) + self.rear_drawable.trans_translate ((0, -0.5)) + self.rear = Rear (self.rear_drawable) def draw (self): - self.arm.draw () - self.rear.draw () + self.arm.draw () + self.rear.draw () class Inter (Frame): """Robot simulation interface.""" @@ -244,54 +244,54 @@ class Inter (Frame): Frame.__init__ (self, master) self.pack (expand = 1, fill = 'both') self.createWidgets () - self.updated = [ ] + self.updated = [ ] def createWidgets (self): - self.rightFrame = Frame (self) - self.rightFrame.pack (side = 'right', fill = 'y') - self.quitButton = Button (self.rightFrame, text = 'Quit', command = self.quit) - self.quitButton.pack (side = 'top') - self.actuatorview = ActuatorView (self.rightFrame) - self.actuatorview.pack (side = 'bottom', fill = 'x') - - self.sensorFrame = Frame (self.rightFrame, borderwidth = 1, relief = - 'sunken') - self.sensorFrame.pack (side = 'bottom', fill = 'x') - self.jackVar = IntVar () - self.jackButton = Checkbutton (self.sensorFrame, text = 'Jack', - indicatoron = False, variable = self.jackVar) - self.jackButton.pack (side = 'top') - self.colorVar = IntVar () - self.colorButton = Checkbutton (self.sensorFrame, text = 'Color', - indicatoron = False, variable = self.colorVar) - self.colorButton.pack (side = 'top') - - self.tableview = TableView (self) - self.tableview.pack (expand = True, fill = 'both') + self.rightFrame = Frame (self) + self.rightFrame.pack (side = 'right', fill = 'y') + self.quitButton = Button (self.rightFrame, text = 'Quit', command = self.quit) + self.quitButton.pack (side = 'top') + self.actuatorview = ActuatorView (self.rightFrame) + self.actuatorview.pack (side = 'bottom', fill = 'x') + + self.sensorFrame = Frame (self.rightFrame, borderwidth = 1, relief = + 'sunken') + self.sensorFrame.pack (side = 'bottom', fill = 'x') + self.jackVar = IntVar () + self.jackButton = Checkbutton (self.sensorFrame, text = 'Jack', + indicatoron = False, variable = self.jackVar) + self.jackButton.pack (side = 'top') + self.colorVar = IntVar () + self.colorButton = Checkbutton (self.sensorFrame, text = 'Color', + indicatoron = False, variable = self.colorVar) + self.colorButton.pack (side = 'top') + + self.tableview = TableView (self) + self.tableview.pack (expand = True, fill = 'both') def update (self, *args): - """If called with arguments, add them to the list of objects to be - updated. - If called without argument, redraw all objects to be updated.""" - if args: - for i in args: - if i not in self.updated: - self.updated.append (i) - else: - for i in self.updated: - i.draw () - self.updated = [ ] + """If called with arguments, add them to the list of objects to be + updated. + If called without argument, redraw all objects to be updated.""" + if args: + for i in args: + if i not in self.updated: + self.updated.append (i) + else: + for i in self.updated: + i.draw () + self.updated = [ ] if __name__ == '__main__': app = Inter () app.tableview.robot.angle = pi / 3 app.tableview.robot.pos = (700, 700) if 0: - from dist_sensor import DistSensor - ds = DistSensor (app.tableview.robot, (150, -127), -pi / 12, 800) - app.tableview.robot.drawn.append (ds) - app.tableview.drawn.append (Obstacle (app.tableview, (1300, 1200), 150)) - ds.obstacles = app.tableview.drawn + from dist_sensor import DistSensor + ds = DistSensor (app.tableview.robot, (150, -127), -pi / 12, 800) + app.tableview.robot.drawn.append (ds) + app.tableview.drawn.append (Obstacle (app.tableview, (1300, 1200), 150)) + ds.obstacles = app.tableview.drawn app.actuatorview.arm.angle = pi/6 app.actuatorview.rear.traps[0].pos = 1 app.actuatorview.rear.traps[1].pos = 0 diff --git a/host/inter/inter_node.py b/host/inter/inter_node.py index 96648c6f..12aaee6e 100644 --- a/host/inter/inter_node.py +++ b/host/inter/inter_node.py @@ -49,160 +49,160 @@ class InterNode (Inter): IO_PATH = 0xb4 def __init__ (self): - Inter.__init__ (self) - self.node = Node () - self.node.register (0xa0, self.handle_asserv_0) - self.node.register (0xa8, self.handle_asserv_8) - self.node.register (self.IO_JACK, self.handle_IO_JACK) - self.node.register (self.IO_COLOR, self.handle_IO_COLOR) - self.node.register (self.IO_SERVO, self.handle_IO_SERVO) - self.node.register (self.IO_SHARPS, self.handle_IO_SHARPS) - self.node.register (self.IO_PATH, self.handle_IO_PATH) - self.tk.createfilehandler (self.node, READABLE, self.read) - self.date = 0 - self.synced = True - self.step_after = None - self.step_time = None - self.obstacles = [ ] - self.dist_sensors = [ - DistSensor (self.tableview.robot, (150, 127), 0, 800), - DistSensor (self.tableview.robot, (150, 0), 0, 800), - DistSensor (self.tableview.robot, (150, -127), 0, 800), - DistSensor (self.tableview.robot, (-70, 100), pi, 800), - DistSensor (self.tableview.robot, (-70, -100), pi, 800), - ] - for s in self.dist_sensors: - s.obstacles = self.obstacles - s.hide = True - self.tableview.robot.drawn.extend (self.dist_sensors) - self.path = Path (self.tableview.table) - self.tableview.drawn.append (self.path) - self.tableview + Inter.__init__ (self) + self.node = Node () + self.node.register (0xa0, self.handle_asserv_0) + self.node.register (0xa8, self.handle_asserv_8) + self.node.register (self.IO_JACK, self.handle_IO_JACK) + self.node.register (self.IO_COLOR, self.handle_IO_COLOR) + self.node.register (self.IO_SERVO, self.handle_IO_SERVO) + self.node.register (self.IO_SHARPS, self.handle_IO_SHARPS) + self.node.register (self.IO_PATH, self.handle_IO_PATH) + self.tk.createfilehandler (self.node, READABLE, self.read) + self.date = 0 + self.synced = True + self.step_after = None + self.step_time = None + self.obstacles = [ ] + self.dist_sensors = [ + DistSensor (self.tableview.robot, (150, 127), 0, 800), + DistSensor (self.tableview.robot, (150, 0), 0, 800), + DistSensor (self.tableview.robot, (150, -127), 0, 800), + DistSensor (self.tableview.robot, (-70, 100), pi, 800), + DistSensor (self.tableview.robot, (-70, -100), pi, 800), + ] + for s in self.dist_sensors: + s.obstacles = self.obstacles + s.hide = True + self.tableview.robot.drawn.extend (self.dist_sensors) + self.path = Path (self.tableview.table) + self.tableview.drawn.append (self.path) + self.tableview def createWidgets (self): - Inter.createWidgets (self) - self.nowLabel = Label (self.rightFrame, text = 'Now: 0 s') - self.nowLabel.pack () - self.stepButton = Button (self.rightFrame, text = 'Step', - command = self.step) - self.stepButton.pack () - self.stepSizeScale = Scale (self.rightFrame, orient = HORIZONTAL, - from_ = 0.05, to = 1.0, resolution = 0.05) - self.stepSizeScale.pack () - self.playVar = IntVar () - self.playButton = Checkbutton (self.rightFrame, variable = - self.playVar, text = 'Play', command = self.play) - self.playButton.pack () - self.tableview.bind ('<2>', self.place_obstacle) - self.showSensorsVar = IntVar () - self.showSensorsButton = Checkbutton (self.sensorFrame, variable = - self.showSensorsVar, text = 'Show sensors', command = - self.show_sensors) - self.showSensorsButton.pack () + Inter.createWidgets (self) + self.nowLabel = Label (self.rightFrame, text = 'Now: 0 s') + self.nowLabel.pack () + self.stepButton = Button (self.rightFrame, text = 'Step', + command = self.step) + self.stepButton.pack () + self.stepSizeScale = Scale (self.rightFrame, orient = HORIZONTAL, + from_ = 0.05, to = 1.0, resolution = 0.05) + self.stepSizeScale.pack () + self.playVar = IntVar () + self.playButton = Checkbutton (self.rightFrame, variable = + self.playVar, text = 'Play', command = self.play) + self.playButton.pack () + self.tableview.bind ('<2>', self.place_obstacle) + self.showSensorsVar = IntVar () + self.showSensorsButton = Checkbutton (self.sensorFrame, variable = + self.showSensorsVar, text = 'Show sensors', command = + self.show_sensors) + self.showSensorsButton.pack () def step (self): - """Do a step. Signal to the Hub we are ready to wait to the next step - date.""" - self.node.wait_async (self.date - + int (self.stepSizeScale.get () * self.TICK)) - self.synced = False - self.step_after = None - self.step_time = time.time () + """Do a step. Signal to the Hub we are ready to wait to the next step + date.""" + self.node.wait_async (self.date + + int (self.stepSizeScale.get () * self.TICK)) + self.synced = False + self.step_after = None + self.step_time = time.time () def play (self): - """Activate auto-steping.""" - if self.playVar.get (): - if self.step_after is None and self.synced: - self.step () - self.stepButton.configure (state = DISABLED) - else: - if self.step_after is not None: - self.after_cancel (self.step_after) - self.step_after = None - self.stepButton.configure (state = NORMAL) + """Activate auto-steping.""" + if self.playVar.get (): + if self.step_after is None and self.synced: + self.step () + self.stepButton.configure (state = DISABLED) + else: + if self.step_after is not None: + self.after_cancel (self.step_after) + self.step_after = None + self.stepButton.configure (state = NORMAL) def read (self, file, mask): - """Handle event on the Node.""" - self.node.read () - if not self.synced and self.node.sync (): - self.synced = True - self.date = self.node.date - self.nowLabel.configure (text = 'Now: %.2f s' % (self.date - / self.TICK)) - self.update () - if self.playVar.get (): - assert self.step_after is None - next = self.step_time + self.stepSizeScale.get () - delay = next - time.time () - if delay > 0: - self.step_after = self.after (int (delay * 1000), - self.step) - else: - self.step () + """Handle event on the Node.""" + self.node.read () + if not self.synced and self.node.sync (): + self.synced = True + self.date = self.node.date + self.nowLabel.configure (text = 'Now: %.2f s' % (self.date + / self.TICK)) + self.update () + if self.playVar.get (): + assert self.step_after is None + next = self.step_time + self.stepSizeScale.get () + delay = next - time.time () + if delay > 0: + self.step_after = self.after (int (delay * 1000), + self.step) + else: + self.step () def handle_asserv_0 (self, msg): - x, y, a = msg.pop ('hhl') - self.tableview.robot.pos = (x, y) - self.tableview.robot.angle = float (a) / 1024 - self.update (self.tableview.robot) + x, y, a = msg.pop ('hhl') + self.tableview.robot.pos = (x, y) + self.tableview.robot.angle = float (a) / 1024 + self.update (self.tableview.robot) def handle_asserv_8 (self, msg): - a, = msg.pop ('l') - self.actuatorview.arm.angle = float (a) / 1024 - self.update (self.actuatorview.arm) + a, = msg.pop ('l') + self.actuatorview.arm.angle = float (a) / 1024 + self.update (self.actuatorview.arm) def handle_IO_JACK (self, msg): - m = Msg (self.IO_JACK) - m.push ('B', self.jackVar.get ()) - self.node.response (m) + m = Msg (self.IO_JACK) + m.push ('B', self.jackVar.get ()) + self.node.response (m) def handle_IO_COLOR (self, msg): - m = Msg (self.IO_COLOR) - m.push ('B', self.colorVar.get ()) - self.node.response (m) + m = Msg (self.IO_COLOR) + m.push ('B', self.colorVar.get ()) + self.node.response (m) def handle_IO_SERVO (self, msg): - for t in self.actuatorview.rear.traps: - t.pos = float (msg.pop ('B')[0]) / 255 - self.update (self.actuatorview.rear) + for t in self.actuatorview.rear.traps: + t.pos = float (msg.pop ('B')[0]) / 255 + self.update (self.actuatorview.rear) def handle_IO_SHARPS (self, msg): - m = Msg (self.IO_SHARPS) - for i in self.dist_sensors: - d = i.distance or 800 - d /= 10 - if d > 10: - v = 0.000571429 * d*d + -0.0752381 * d + 2.89107 - else: - v = 2.2 / 10 * d - v *= 1024 / 5 - m.push ('H', v) - assert v >= 0 and v < 1024 - self.node.response (m) + m = Msg (self.IO_SHARPS) + for i in self.dist_sensors: + d = i.distance or 800 + d /= 10 + if d > 10: + v = 0.000571429 * d*d + -0.0752381 * d + 2.89107 + else: + v = 2.2 / 10 * d + v *= 1024 / 5 + m.push ('H', v) + assert v >= 0 and v < 1024 + self.node.response (m) def handle_IO_PATH (self, msg): - self.path.path = [ ] - while len (msg) >= 4: - self.path.path.append (msg.pop ('hh')) - self.update (self.path) + self.path.path = [ ] + while len (msg) >= 4: + self.path.path.append (msg.pop ('hh')) + self.update (self.path) def place_obstacle (self, ev): - pos = self.tableview.screen_coord ((ev.x, ev.y)) - if self.obstacles: - self.obstacles[0].pos = pos - else: - self.obstacles.append (Obstacle (self.tableview.table, pos, 150)) - self.tableview.drawn.append (self.obstacles[0]) - self.update (*self.obstacles) - self.update (*self.dist_sensors) - self.update () + pos = self.tableview.screen_coord ((ev.x, ev.y)) + if self.obstacles: + self.obstacles[0].pos = pos + else: + self.obstacles.append (Obstacle (self.tableview.table, pos, 150)) + self.tableview.drawn.append (self.obstacles[0]) + self.update (*self.obstacles) + self.update (*self.dist_sensors) + self.update () def show_sensors (self): - hide = not self.showSensorsVar.get () - for i in self.dist_sensors: - i.hide = hide - self.update (*self.dist_sensors) - self.update () + hide = not self.showSensorsVar.get () + for i in self.dist_sensors: + i.hide = hide + self.update (*self.dist_sensors) + self.update () if __name__ == '__main__': import mex.hub @@ -210,9 +210,9 @@ if __name__ == '__main__': h = mex.hub.Hub (min_clients = 1) fh = utils.forked.Forked (h.wait) try: - app = InterNode () - app.mainloop() + app = InterNode () + app.mainloop() finally: - fh.kill () - import time - time.sleep (1) + fh.kill () + import time + time.sleep (1) diff --git a/host/inter/path.py b/host/inter/path.py index 76db970a..eb5744eb 100644 --- a/host/inter/path.py +++ b/host/inter/path.py @@ -32,11 +32,11 @@ class Path (Drawable): """Computed path drawing.""" def __init__ (self, onto): - Drawable.__init__ (self, onto) - self.path = [ ] + Drawable.__init__ (self, onto) + self.path = [ ] def draw (self): - self.reset () - if len (self.path) > 2: - fmt = dict (fill = 'green', arrow = LAST) - self.draw_line (*self.path, **fmt) + self.reset () + if len (self.path) > 2: + fmt = dict (fill = 'green', arrow = LAST) + self.draw_line (*self.path, **fmt) diff --git a/host/inter/test/test_drawable.py b/host/inter/test/test_drawable.py index 79948e12..6f86dd2d 100644 --- a/host/inter/test/test_drawable.py +++ b/host/inter/test/test_drawable.py @@ -7,38 +7,38 @@ from math import pi class Test (Drawable): def draw (self): - self.draw_rectangle ((-100, -100), (100, 100), fill = '', outline = 'gray') - self.draw_rectangle ((0, 0), (5, 5), fill = 'red') - self.draw_rectangle ((20, 20), (50, 50), fill = '', outline = 'blue') - self.draw_line ((20, 20), (25, 25), (80, 0), (0, 80), fill = 'green') - self.draw_line ((20, 20), (25, 25), (80, 0), (0, 80), smooth = True) - self.draw_circle ((40, -40), 10) - self.draw_arc ((-40, 0), 20, start = pi / 4, extent = pi / 2) + self.draw_rectangle ((-100, -100), (100, 100), fill = '', outline = 'gray') + self.draw_rectangle ((0, 0), (5, 5), fill = 'red') + self.draw_rectangle ((20, 20), (50, 50), fill = '', outline = 'blue') + self.draw_line ((20, 20), (25, 25), (80, 0), (0, 80), fill = 'green') + self.draw_line ((20, 20), (25, 25), (80, 0), (0, 80), smooth = True) + self.draw_circle ((40, -40), 10) + self.draw_arc ((-40, 0), 20, start = pi / 4, extent = pi / 2) class App (DrawableCanvas): def __init__ (self, master = None): - DrawableCanvas.__init__ (self, 300, 300, 20, 20, master) - self.pack (expand = True, fill = 'both') - self.test = Test (self) - self.animated = False - self.i = 0 + DrawableCanvas.__init__ (self, 300, 300, 20, 20, master) + self.pack (expand = True, fill = 'both') + self.test = Test (self) + self.animated = False + self.i = 0 def animate (self): - # Real user should reset at each redraw. - self.after (500, self.animate) - self.test.draw () - self.test.trans_rotate (-pi/12) - self.test.trans_translate ((10, 10)) - self.test.trans_scale (1.05) - self.i += 1 - if self.i == 10: - self.test.reset () + # Real user should reset at each redraw. + self.after (500, self.animate) + self.test.draw () + self.test.trans_rotate (-pi/12) + self.test.trans_translate ((10, 10)) + self.test.trans_scale (1.05) + self.i += 1 + if self.i == 10: + self.test.reset () def draw (self): - if not self.animated: - self.animate () - self.animated = True + if not self.animated: + self.animate () + self.animated = True app = App () app.mainloop () diff --git a/host/inter/trans_matrix.py b/host/inter/trans_matrix.py index dcaab134..a8f3849a 100644 --- a/host/inter/trans_matrix.py +++ b/host/inter/trans_matrix.py @@ -33,138 +33,138 @@ class TransMatrix: IDENTITY = ((1, 0), (0, 1), (0, 0)) def __init__ (self, *m): - """Initialise the matrix, with optional initial value. - - >>> TransMatrix () - ((1, 0), (0, 1), (0, 0)) - >>> TransMatrix ((1, 2), (3, 4), (5, 6)) - ((1, 2), (3, 4), (5, 6)) - """ - if m: - assert len (m) == 3 - for i in m: - assert len (i) == 2 - self.matrix = m - else: - self.matrix = self.IDENTITY + """Initialise the matrix, with optional initial value. + + >>> TransMatrix () + ((1, 0), (0, 1), (0, 0)) + >>> TransMatrix ((1, 2), (3, 4), (5, 6)) + ((1, 2), (3, 4), (5, 6)) + """ + if m: + assert len (m) == 3 + for i in m: + assert len (i) == 2 + self.matrix = m + else: + self.matrix = self.IDENTITY def identity (self): - """Set to identity. + """Set to identity. - >>> a = TransMatrix () - >>> a.translate ((2, 3)) - >>> a.identity (); a - ((1, 0), (0, 1), (0, 0)) - """ - self.matrix = self.IDENTITY + >>> a = TransMatrix () + >>> a.translate ((2, 3)) + >>> a.identity (); a + ((1, 0), (0, 1), (0, 0)) + """ + self.matrix = self.IDENTITY def rotate (self, angle): - """Transform the current matrix to do a rotation. - - >>> from math import pi - >>> a = TransMatrix () - >>> a.rotate (pi / 3); a # doctest: +ELLIPSIS - ((0.5..., 0.866...), (-0.866..., 0.5...), (0.0, 0.0)) - """ - s = sin (angle) - c = cos (angle) - m = TransMatrix ((c, s), (-s, c), (0, 0)) - self *= m + """Transform the current matrix to do a rotation. + + >>> from math import pi + >>> a = TransMatrix () + >>> a.rotate (pi / 3); a # doctest: +ELLIPSIS + ((0.5..., 0.866...), (-0.866..., 0.5...), (0.0, 0.0)) + """ + s = sin (angle) + c = cos (angle) + m = TransMatrix ((c, s), (-s, c), (0, 0)) + self *= m def translate (self, by): - """Transform the current matrix to do a translation. - - >>> a = TransMatrix () - >>> a.translate ((2, 3)); a - ((1, 0), (0, 1), (2, 3)) - """ - m = TransMatrix ((1, 0), (0, 1), by) - self *= m + """Transform the current matrix to do a translation. + + >>> a = TransMatrix () + >>> a.translate ((2, 3)); a + ((1, 0), (0, 1), (2, 3)) + """ + m = TransMatrix ((1, 0), (0, 1), by) + self *= m def scale (self, factor): - """Transform the current matrix to do a scaling. - - >>> a = TransMatrix () - >>> a.scale (2); a - ((2, 0), (0, 2), (0, 0)) - """ - m = TransMatrix ((factor, 0), (0, factor), (0, 0)) - self *= m + """Transform the current matrix to do a scaling. + + >>> a = TransMatrix () + >>> a.scale (2); a + ((2, 0), (0, 2), (0, 0)) + """ + m = TransMatrix ((factor, 0), (0, factor), (0, 0)) + self *= m def __imul__ (self, other): - """Multiply by an other matrix. - - >>> a = TransMatrix ((1, 0), (0, 1), (1, 0)) - >>> b = TransMatrix ((0, 1), (1, 0), (0, 1)) - >>> a *= b; a - ((0, 1), (1, 0), (0, 2)) - """ - s = self.matrix - o = other.matrix - self.matrix = ( - (s[0][0] * o[0][0] + s[0][1] * o[1][0], - s[0][0] * o[0][1] + s[0][1] * o[1][1]), - (s[1][0] * o[0][0] + s[1][1] * o[1][0], - s[1][0] * o[0][1] + s[1][1] * o[1][1]), - (s[2][0] * o[0][0] + s[2][1] * o[1][0] + o[2][0], - s[2][0] * o[0][1] + s[2][1] * o[1][1] + o[2][1])) - return self + """Multiply by an other matrix. + + >>> a = TransMatrix ((1, 0), (0, 1), (1, 0)) + >>> b = TransMatrix ((0, 1), (1, 0), (0, 1)) + >>> a *= b; a + ((0, 1), (1, 0), (0, 2)) + """ + s = self.matrix + o = other.matrix + self.matrix = ( + (s[0][0] * o[0][0] + s[0][1] * o[1][0], + s[0][0] * o[0][1] + s[0][1] * o[1][1]), + (s[1][0] * o[0][0] + s[1][1] * o[1][0], + s[1][0] * o[0][1] + s[1][1] * o[1][1]), + (s[2][0] * o[0][0] + s[2][1] * o[1][0] + o[2][0], + s[2][0] * o[0][1] + s[2][1] * o[1][1] + o[2][1])) + return self def apply (self, *args): - """Apply (multiply) the matrix to all the given arguments. - - >>> m = TransMatrix ((1, 2), (4, 8), (16, 32)) - >>> m.apply ((1, 0)) - (17, 34) - >>> m.apply ((0, 1), (1, 1)) - ((20, 40), (21, 42)) - """ - r = tuple ( - (i[0] * self.matrix[0][0] + i[1] * self.matrix[1][0] - + self.matrix[2][0], - i[0] * self.matrix[0][1] + i[1] * self.matrix[1][1] - + self.matrix[2][1]) - for i in args) - if len (args) == 1: - return r[0] - else: - return r + """Apply (multiply) the matrix to all the given arguments. + + >>> m = TransMatrix ((1, 2), (4, 8), (16, 32)) + >>> m.apply ((1, 0)) + (17, 34) + >>> m.apply ((0, 1), (1, 1)) + ((20, 40), (21, 42)) + """ + r = tuple ( + (i[0] * self.matrix[0][0] + i[1] * self.matrix[1][0] + + self.matrix[2][0], + i[0] * self.matrix[0][1] + i[1] * self.matrix[1][1] + + self.matrix[2][1]) + for i in args) + if len (args) == 1: + return r[0] + else: + return r def apply_angle (self, angle): - """Apply the matrix to an angle. - - >>> from math import pi - >>> a = TransMatrix () - >>> a.rotate (pi / 6) - >>> a.translate ((2, 3)) - >>> a.scale (4) - >>> a.apply_angle (pi / 6), pi / 3 # doctest: +ELLIPSIS - (1.0471..., 1.0471...) - """ - o, m = self.apply ((0, 0), (cos (angle), sin (angle))) - v = (m[0] - o[0], m[1] - o[1]) - vl = sqrt (v[0] ** 2 + v[1] ** 2) - v = (v[0] / vl, v[1] / vl) - return atan2 (v[1], v[0]) + """Apply the matrix to an angle. + + >>> from math import pi + >>> a = TransMatrix () + >>> a.rotate (pi / 6) + >>> a.translate ((2, 3)) + >>> a.scale (4) + >>> a.apply_angle (pi / 6), pi / 3 # doctest: +ELLIPSIS + (1.0471..., 1.0471...) + """ + o, m = self.apply ((0, 0), (cos (angle), sin (angle))) + v = (m[0] - o[0], m[1] - o[1]) + vl = sqrt (v[0] ** 2 + v[1] ** 2) + v = (v[0] / vl, v[1] / vl) + return atan2 (v[1], v[0]) def apply_distance (self, distance): - """Apply the matrix to a distance. - - >>> from math import pi - >>> a = TransMatrix () - >>> a.rotate (pi / 6) - >>> a.translate ((2, 3)) - >>> a.scale (4) - >>> round (a.apply_distance (2)) - 8.0 - """ - o, m = self.apply ((0, 0), (distance, 0)) - v = (m[0] - o[0], m[1] - o[1]) - vl = sqrt (v[0] ** 2 + v[1] ** 2) - return vl + """Apply the matrix to a distance. + + >>> from math import pi + >>> a = TransMatrix () + >>> a.rotate (pi / 6) + >>> a.translate ((2, 3)) + >>> a.scale (4) + >>> round (a.apply_distance (2)) + 8.0 + """ + o, m = self.apply ((0, 0), (distance, 0)) + v = (m[0] - o[0], m[1] - o[1]) + vl = sqrt (v[0] ** 2 + v[1] ** 2) + return vl def __repr__ (self): - return self.matrix.__repr__ () + return self.matrix.__repr__ () def _test (): import doctest diff --git a/host/mex/hub.py b/host/mex/hub.py index 5ae88ba7..4dd796a4 100644 --- a/host/mex/hub.py +++ b/host/mex/hub.py @@ -30,152 +30,152 @@ from struct import pack, unpack, calcsize class Hub: def __init__ (self, addr = mex.DEFAULT_ADDR, min_clients = 0, log = None): - """Initialise a new Hub and bind to server address.""" - self.addr = addr - self.min_clients = min_clients - self.log = log - self.clients = { } - self.next_client_id = 1 - self.date = 0 - self.socket = socket.socket () - self.socket.setsockopt (socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) - self.socket.setsockopt (socket.SOL_TCP, socket.TCP_NODELAY, 1) - self.socket.bind (self.addr) - self.socket.listen (5) + """Initialise a new Hub and bind to server address.""" + self.addr = addr + self.min_clients = min_clients + self.log = log + self.clients = { } + self.next_client_id = 1 + self.date = 0 + self.socket = socket.socket () + self.socket.setsockopt (socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + self.socket.setsockopt (socket.SOL_TCP, socket.TCP_NODELAY, 1) + self.socket.bind (self.addr) + self.socket.listen (5) def wait (self, cond = False): - """Wait until a cond is True and handle message exchanges.""" - while not cond and self.date != None: - self.select () + """Wait until a cond is True and handle message exchanges.""" + while not cond and self.date != None: + self.select () def select (self): - """Wait until the next event (connection or message) occurs and handle - it.""" - if not self.min_clients: - # Check idle and wanted dates. - idle = True - min_wait_date = None - for c in self.clients.itervalues (): - if not c.idle: - idle = False - break - if c.wait_date != None and (c.wait_date < min_wait_date or - min_wait_date == None): - min_wait_date = c.wait_date - # Abort if everyone waits forever. - if idle and min_wait_date == None: - self.date = None - return - # Send new date. - if idle: - self.date = min_wait_date - if self.log: - self.log ('[%d] date' % self.date) - date = Msg (mex.DATE) - date.push ('L', self.date) - for c in self.clients.itervalues (): - c.send (date) - # Prepare fdset and select. - infds = [ self.socket ] - if not self.min_clients: - infds += self.clients.values () - readyfds = select.select (infds, (), ()) [0] - for i in readyfds: - if i is self.socket: - self.accept () - else: - i.read () + """Wait until the next event (connection or message) occurs and handle + it.""" + if not self.min_clients: + # Check idle and wanted dates. + idle = True + min_wait_date = None + for c in self.clients.itervalues (): + if not c.idle: + idle = False + break + if c.wait_date != None and (c.wait_date < min_wait_date or + min_wait_date == None): + min_wait_date = c.wait_date + # Abort if everyone waits forever. + if idle and min_wait_date == None: + self.date = None + return + # Send new date. + if idle: + self.date = min_wait_date + if self.log: + self.log ('[%d] date' % self.date) + date = Msg (mex.DATE) + date.push ('L', self.date) + for c in self.clients.itervalues (): + c.send (date) + # Prepare fdset and select. + infds = [ self.socket ] + if not self.min_clients: + infds += self.clients.values () + readyfds = select.select (infds, (), ()) [0] + for i in readyfds: + if i is self.socket: + self.accept () + else: + i.read () def accept (self): - """Accept a new connection and create a client.""" - # Add client. - a = self.socket.accept () - if self.log: - self.log ('[%d] connect from %s' % (self.date, str (a[1]))) - c = Hub.Client (self, a[0], self.next_client_id) - self.next_client_id += 1 - assert self.next_client_id < 256 - self.clients[c.id] = c - if self.min_clients: - self.min_clients -= 1 - # Send first date. - date = Msg (mex.DATE) - date.push ('L', self.date) - c.send (date) + """Accept a new connection and create a client.""" + # Add client. + a = self.socket.accept () + if self.log: + self.log ('[%d] connect from %s' % (self.date, str (a[1]))) + c = Hub.Client (self, a[0], self.next_client_id) + self.next_client_id += 1 + assert self.next_client_id < 256 + self.clients[c.id] = c + if self.min_clients: + self.min_clients -= 1 + # Send first date. + date = Msg (mex.DATE) + date.push ('L', self.date) + c.send (date) def broadcast (self, msg, exclude = None): - """Send a message broadcasted, could exclude a client.""" - for c in self.clients.itervalues (): - if c is not exclude: - c.send (msg) + """Send a message broadcasted, could exclude a client.""" + for c in self.clients.itervalues (): + if c is not exclude: + c.send (msg) class Client: - def __init__ (self, hub, socket, id): - """Initialise a new client.""" - self.hub = hub - self.socket = socket - self.id = id - self.seq = 0 - self.idle = True - self.wait_date = None + def __init__ (self, hub, socket, id): + """Initialise a new client.""" + self.hub = hub + self.socket = socket + self.id = id + self.seq = 0 + self.idle = True + self.wait_date = None - def read (self): - """Read and dispatch a message from this client.""" - # Read message. - head = self.socket.recv (calcsize (mex.HEADER_FMT)) - if head == '': - self.socket.close () - del self.hub.clients[self.id] - return - size, seq = unpack (mex.HEADER_FMT, head) - data = self.socket.recv (size) - m = Msg (data) - if self.hub.log: - self.hub.log ('[%d] received from %d(%d): %s' % - (self.hub.date, self.id, seq, str (m))) - # Dispatch. - if m.mtype == mex.IDLE: - if seq == self.seq: - self.idle = True - if len (m): - self.wait_date, = m.pop ('L') - assert self.wait_date >= self.hub.date - else: - self.wait_date = None - elif m.mtype == mex.DATE: - date = Msg (mex.DATE) - date.push ('L', self.hub.date) - self.send (date) - elif m.mtype == mex.REQ: - m.pop ('B') - mr = Msg (mex.REQ) - mr.push ('B', self.id) - mr.push (m.pop ()) - self.hub.broadcast (mr, self) - elif m.mtype == mex.RSP: - to, = m.pop ('B') - mr = Msg (mex.RSP) - mr.push ('B', 0) - mr.push (m.pop ()) - self.hub.clients[to].send (mr) - else: - self.hub.broadcast (m, self) + def read (self): + """Read and dispatch a message from this client.""" + # Read message. + head = self.socket.recv (calcsize (mex.HEADER_FMT)) + if head == '': + self.socket.close () + del self.hub.clients[self.id] + return + size, seq = unpack (mex.HEADER_FMT, head) + data = self.socket.recv (size) + m = Msg (data) + if self.hub.log: + self.hub.log ('[%d] received from %d(%d): %s' % + (self.hub.date, self.id, seq, str (m))) + # Dispatch. + if m.mtype == mex.IDLE: + if seq == self.seq: + self.idle = True + if len (m): + self.wait_date, = m.pop ('L') + assert self.wait_date >= self.hub.date + else: + self.wait_date = None + elif m.mtype == mex.DATE: + date = Msg (mex.DATE) + date.push ('L', self.hub.date) + self.send (date) + elif m.mtype == mex.REQ: + m.pop ('B') + mr = Msg (mex.REQ) + mr.push ('B', self.id) + mr.push (m.pop ()) + self.hub.broadcast (mr, self) + elif m.mtype == mex.RSP: + to, = m.pop ('B') + mr = Msg (mex.RSP) + mr.push ('B', 0) + mr.push (m.pop ()) + self.hub.clients[to].send (mr) + else: + self.hub.broadcast (m, self) - def send (self, msg): - """Send a message to this client.""" - data = msg.data () - self.seq += 1 - if self.seq == 256: - self.seq = 0 - packet = pack (mex.HEADER_FMT, len (data), self.seq) + data - self.socket.sendall (packet) - self.idle = False - if self.hub.log: - self.hub.log ('[%d] sending to %d(%d): %s' % (self.hub.date, - self.id, self.seq, str (msg))) + def send (self, msg): + """Send a message to this client.""" + data = msg.data () + self.seq += 1 + if self.seq == 256: + self.seq = 0 + packet = pack (mex.HEADER_FMT, len (data), self.seq) + data + self.socket.sendall (packet) + self.idle = False + if self.hub.log: + self.hub.log ('[%d] sending to %d(%d): %s' % (self.hub.date, + self.id, self.seq, str (msg))) - def fileno (self): - """Return socket filedescriptor.""" - return self.socket.fileno () + def fileno (self): + """Return socket filedescriptor.""" + return self.socket.fileno () diff --git a/host/mex/msg.py b/host/mex/msg.py index 5399a3c9..0cd01c8c 100644 --- a/host/mex/msg.py +++ b/host/mex/msg.py @@ -84,64 +84,64 @@ class Msg: """ def __init__ (self, f): - """Initialise a new message, see class documentation for - signatures.""" - try: - f[0] - except TypeError: - # New empty message. - self.mtype = f - self.header = pack ('!B', self.mtype) - self.payload = '' - else: - # Read from a buffer. - self.header = f[0:1] - self.payload = f[1:] - self.mtype = unpack ('!B', self.header)[0] + """Initialise a new message, see class documentation for + signatures.""" + try: + f[0] + except TypeError: + # New empty message. + self.mtype = f + self.header = pack ('!B', self.mtype) + self.payload = '' + else: + # Read from a buffer. + self.header = f[0:1] + self.payload = f[1:] + self.mtype = unpack ('!B', self.header)[0] def data (self): - """Get the message data, ready to be sent.""" - return self.header + self.payload + """Get the message data, ready to be sent.""" + return self.header + self.payload def __str__ (self): - """Return an text representation.""" - payload = ' '.join (['%02x' % ord (i) for i in self.payload]) - return '' % (self.mtype, payload) + """Return an text representation.""" + payload = ' '.join (['%02x' % ord (i) for i in self.payload]) + return '' % (self.mtype, payload) def push (self, fmt, *args): - """ - Add data to the payload. - - msg.push (string) -> None. Add the given string to the payload. - - msg.push (fmt, values...) -> None. Add the given values to the - payload, using a struct.pack format string. - """ - if args: - self.payload += pack ('!' + fmt, *args) - else: - self.payload += fmt + """ + Add data to the payload. + + msg.push (string) -> None. Add the given string to the payload. + + msg.push (fmt, values...) -> None. Add the given values to the + payload, using a struct.pack format string. + """ + if args: + self.payload += pack ('!' + fmt, *args) + else: + self.payload += fmt def pop (self, fmt = None): - """ - Get data from the payload. - - msg.pop () -> payload. Get all the remaining payload. - - msg.pop (fmt) -> (values, ...). Get values extracted according to a - struct.unpack format string. - """ - if fmt: - nb = calcsize (fmt) - ex, self.payload = self.payload[0:nb], self.payload[nb:] - return unpack ('!' + fmt, ex) - else: - ex, self.payload = self.payload, '' - return ex + """ + Get data from the payload. + + msg.pop () -> payload. Get all the remaining payload. + + msg.pop (fmt) -> (values, ...). Get values extracted according to a + struct.unpack format string. + """ + if fmt: + nb = calcsize (fmt) + ex, self.payload = self.payload[0:nb], self.payload[nb:] + return unpack ('!' + fmt, ex) + else: + ex, self.payload = self.payload, '' + return ex def __len__ (self): - """Get payload remaining length.""" - return len (self.payload) + """Get payload remaining length.""" + return len (self.payload) def _test (): import doctest diff --git a/host/mex/node.py b/host/mex/node.py index 3307a4d7..2a6a5d22 100644 --- a/host/mex/node.py +++ b/host/mex/node.py @@ -30,128 +30,128 @@ from struct import pack, unpack, calcsize class Node: class closed: - """Raised on closed connection.""" - pass + """Raised on closed connection.""" + pass def __init__ (self, addr = mex.DEFAULT_ADDR): - """Create a new Node and connect it to given Hub address.""" - self.socket = socket.socket () - self.socket.setsockopt (socket.SOL_TCP, socket.TCP_NODELAY, 1) - self.socket.connect (addr) - self.date = 0 - self.seq = 0 - self.req = None - self.handlers = { } - self.register (mex.DATE, lambda msg: self.handle_DATE (msg)) - self.register (mex.REQ, lambda msg: self.handle_REQ (msg)) - # Synchronise. - rsp = None - while rsp == None or rsp.mtype != mex.DATE: - rsp = self.recv () - self.dispatch (rsp) + """Create a new Node and connect it to given Hub address.""" + self.socket = socket.socket () + self.socket.setsockopt (socket.SOL_TCP, socket.TCP_NODELAY, 1) + self.socket.connect (addr) + self.date = 0 + self.seq = 0 + self.req = None + self.handlers = { } + self.register (mex.DATE, lambda msg: self.handle_DATE (msg)) + self.register (mex.REQ, lambda msg: self.handle_REQ (msg)) + # Synchronise. + rsp = None + while rsp == None or rsp.mtype != mex.DATE: + rsp = self.recv () + self.dispatch (rsp) def wait (self, date = None): - """Wait forever or until a date is reached.""" - while date == None or self.date != date: - idle = Msg (mex.IDLE) - if date != None: - idle.push ('L', date) - self.send (idle) - msg = self.recv () - self.dispatch (msg) + """Wait forever or until a date is reached.""" + while date == None or self.date != date: + idle = Msg (mex.IDLE) + if date != None: + idle.push ('L', date) + self.send (idle) + msg = self.recv () + self.dispatch (msg) def wait_async (self, date = None): - """Asynchronous version of wait. This should not be called again - until sync return True.""" - self.async_waited = date - synced = self.sync () - assert not synced + """Asynchronous version of wait. This should not be called again + until sync return True.""" + self.async_waited = date + synced = self.sync () + assert not synced def sync (self): - """To be called after read or wait_async. Return True if the waited - date is reached or signal the Hub our waiting status.""" - if self.date == self.async_waited: - return True - else: - idle = Msg (mex.IDLE) - if self.async_waited != None: - idle.push ('L', self.async_waited) - self.send (idle) + """To be called after read or wait_async. Return True if the waited + date is reached or signal the Hub our waiting status.""" + if self.date == self.async_waited: + return True + else: + idle = Msg (mex.IDLE) + if self.async_waited != None: + idle.push ('L', self.async_waited) + self.send (idle) def read (self): - """Used for asynchronous operations. Handle incoming data. The sync - method should be called after this one returns.""" - msg = self.recv () - self.dispatch (msg) + """Used for asynchronous operations. Handle incoming data. The sync + method should be called after this one returns.""" + msg = self.recv () + self.dispatch (msg) def send (self, msg): - """Send a message.""" - data = msg.data () - packet = pack (mex.HEADER_FMT, len (data), self.seq) + data - self.socket.sendall (packet) + """Send a message.""" + data = msg.data () + packet = pack (mex.HEADER_FMT, len (data), self.seq) + data + self.socket.sendall (packet) def request (self, msg): - """Send a request and return response.""" - # Send request. - req = Msg (mex.REQ) - req.push ('B', 0) - req.push (msg.data ()) - self.send (req) - # Wait for response. - rsp = self.recv () - while rsp.mtype != mex.RSP: - self.dispatch (rsp) - rsp = self.recv () - # Discard reqid. - rsp.pop ('B') - return Msg (rsp.pop ()) + """Send a request and return response.""" + # Send request. + req = Msg (mex.REQ) + req.push ('B', 0) + req.push (msg.data ()) + self.send (req) + # Wait for response. + rsp = self.recv () + while rsp.mtype != mex.RSP: + self.dispatch (rsp) + rsp = self.recv () + # Discard reqid. + rsp.pop ('B') + return Msg (rsp.pop ()) def response (self, msg): - """Send a response to the currently serviced request.""" - assert self.req != None - rsp = Msg (mex.RSP) - rsp.push ('B', self.req) - self.req = None - rsp.push (msg.data ()) - self.send (rsp) + """Send a response to the currently serviced request.""" + assert self.req != None + rsp = Msg (mex.RSP) + rsp.push ('B', self.req) + self.req = None + rsp.push (msg.data ()) + self.send (rsp) def register (self, mtype, handler): - """Register an handler for the given message type.""" - assert mtype not in self.handlers - self.handlers[mtype] = handler + """Register an handler for the given message type.""" + assert mtype not in self.handlers + self.handlers[mtype] = handler def close (self): - """Close connection with the Hub.""" - self.socket.close () - self.socket = None + """Close connection with the Hub.""" + self.socket.close () + self.socket = None def fileno (self): - """Return socket fileno () for asynchronous operations.""" - return self.socket.fileno () + """Return socket fileno () for asynchronous operations.""" + return self.socket.fileno () def recv (self): - """Receive one message.""" - head = self.socket.recv (calcsize (mex.HEADER_FMT)) - if head == '': - self.close () - raise Node.closed - size, self.seq = unpack (mex.HEADER_FMT, head) - data = self.socket.recv (size) - return Msg (data) + """Receive one message.""" + head = self.socket.recv (calcsize (mex.HEADER_FMT)) + if head == '': + self.close () + raise Node.closed + size, self.seq = unpack (mex.HEADER_FMT, head) + data = self.socket.recv (size) + return Msg (data) def dispatch (self, msg): - """Call the right handler for the given message.""" - if msg.mtype in self.handlers: - self.handlers[msg.mtype] (msg) + """Call the right handler for the given message.""" + if msg.mtype in self.handlers: + self.handlers[msg.mtype] (msg) def handle_DATE (self, msg): - """Handle an incoming DATE.""" - self.date, = msg.pop ('L') + """Handle an incoming DATE.""" + self.date, = msg.pop ('L') def handle_REQ (self, msg): - """Handle an incoming REQ.""" - self.req, = msg.pop ('B') - dec = Msg (msg.pop ()) - self.dispatch (dec) - self.req = None + """Handle an incoming REQ.""" + self.req, = msg.pop ('B') + dec = Msg (msg.pop ()) + self.dispatch (dec) + self.req = None diff --git a/host/mex/test/test.py b/host/mex/test/test.py index c4382c2c..4be24f49 100644 --- a/host/mex/test/test.py +++ b/host/mex/test/test.py @@ -15,12 +15,12 @@ h = Hub (min_clients = 2, log = log) def c1 (): n = Node () def a (msg): - print 'oucouc' - nb, = msg.pop ('B') - nb += 1 - m = Msg (msg.mtype) - m.push ('B', nb) - n.response (m) + print 'oucouc' + nb, = msg.pop ('B') + nb += 1 + m = Msg (msg.mtype) + m.push ('B', nb) + n.response (m) n.register (0x82, a) m = Msg (0x81) n.send (m) @@ -31,7 +31,7 @@ f1 = Forked (c1) def c2 (): n = Node () def a (msg): - print 'coucou' + print 'coucou' n.register (0x81, a) m = Msg (0x82) m.push ('B', 42) @@ -40,9 +40,9 @@ def c2 (): assert r.pop ('B') == (43,) n.wait_async (42) while not n.sync (): - fds = select.select ((n, ), (), ())[0] - for i in fds: - i.read () + fds = select.select ((n, ), (), ())[0] + for i in fds: + i.read () n.wait () f2 = Forked (c2) diff --git a/host/proto/popen_io.py b/host/proto/popen_io.py index 235151d3..0f582802 100644 --- a/host/proto/popen_io.py +++ b/host/proto/popen_io.py @@ -29,22 +29,22 @@ class PopenIO: convert from and to \r and \n.""" def __init__ (self, cmd): - """Initialise and start the given commande line.""" - fout, fin = os.popen2 (cmd, 'b', 0) - time.sleep (0.2) - self.fin = fin - self.fout = fout + """Initialise and start the given commande line.""" + fout, fin = os.popen2 (cmd, 'b', 0) + time.sleep (0.2) + self.fin = fin + self.fout = fout def read (self, *args): - buf = self.fin.read (*args).replace ('\n', '\r') - return buf + buf = self.fin.read (*args).replace ('\n', '\r') + return buf def write (self, *args): - return self.fout.write (*[i.replace ('\r', '\n') for i in args]) + return self.fout.write (*[i.replace ('\r', '\n') for i in args]) def fileno (self): - return self.fin.fileno () + return self.fin.fileno () def close (self): - self.fin.close () - self.fout.close () + self.fin.close () + self.fout.close () diff --git a/host/proto/proto.py b/host/proto/proto.py index 9eb401e1..19dff792 100644 --- a/host/proto/proto.py +++ b/host/proto/proto.py @@ -32,149 +32,149 @@ ARG = 3 class Proto: def __init__ (self, file, date, timeout, log = None): - """Initialise and set file (serial port, pty, socket...), date - function and timeout value. - - - file: open file connected to the slave device. - - date: when called, should return the current time. - - timeout: time after which retransmission is done. - - log: if defined, will be called with a log string. - - """ - self.file = file - self.date = date - self.last_send = None - self.timeout = timeout - self.send_queue = [ ] - self.state = START - self.log = log - self.handlers = { } + """Initialise and set file (serial port, pty, socket...), date + function and timeout value. + + - file: open file connected to the slave device. + - date: when called, should return the current time. + - timeout: time after which retransmission is done. + - log: if defined, will be called with a log string. + + """ + self.file = file + self.date = date + self.last_send = None + self.timeout = timeout + self.send_queue = [ ] + self.state = START + self.log = log + self.handlers = { } def send (self, *frame): - """Queue a frame to send.""" - if not self.send_queue: - self.last_send = None - self.send_queue.append (Frame (*frame)) + """Queue a frame to send.""" + if not self.send_queue: + self.last_send = None + self.send_queue.append (Frame (*frame)) def read (self): - """Read from file and receive frames.""" - for f in self.recv (): - if self.log: - self.log ('recv %s' % f) - if self.send_queue and f == self.send_queue[0]: - del self.send_queue[0] - if self.send_queue: - self.send_head () - else: - self.dispatch (f) + """Read from file and receive frames.""" + for f in self.recv (): + if self.log: + self.log ('recv %s' % f) + if self.send_queue and f == self.send_queue[0]: + del self.send_queue[0] + if self.send_queue: + self.send_head () + else: + self.dispatch (f) def sync (self): - """Send frames, return True if all is sent.""" - if self.send_queue and (self.last_send is None - or self.last_send + self.timeout < self.date ()): - self.send_head () - return not self.send_queue + """Send frames, return True if all is sent.""" + if self.send_queue and (self.last_send is None + or self.last_send + self.timeout < self.date ()): + self.send_head () + return not self.send_queue def wait (self, cond = None): - """Wait forever or until cond () is True.""" - while not (self.sync () and (cond is not None or cond ())): - fds = select.select ((self,), (), (), self.timeout)[0] - for i in fds: - assert i is self - i.read () + """Wait forever or until cond () is True.""" + while not (self.sync () and (cond is not None or cond ())): + fds = select.select ((self,), (), (), self.timeout)[0] + for i in fds: + assert i is self + i.read () def register (self, command, fmt, handler): - """Register a handler for the specified command and format. The - handler will receive decoded arguments.""" - key = (command, struct.calcsize ('!' + fmt)) - assert key not in self.handlers - self.handlers[key] = (handler, fmt) + """Register a handler for the specified command and format. The + handler will receive decoded arguments.""" + key = (command, struct.calcsize ('!' + fmt)) + assert key not in self.handlers + self.handlers[key] = (handler, fmt) def fileno (self): - """Return file descriptor, for use with select.""" - return self.file.fileno () + """Return file descriptor, for use with select.""" + return self.file.fileno () def send_head (self): - """Send first frame from the send queue.""" - if self.log: - self.log ('send %s' % self.send_queue[0]) - self.file.write (self.send_queue[0].data ()) - self.last_send = self.date () + """Send first frame from the send queue.""" + if self.log: + self.log ('send %s' % self.send_queue[0]) + self.file.write (self.send_queue[0].data ()) + self.last_send = self.date () def recv (self): - """Receive a frame, used as a generator.""" - for c in self.file.read (1): - if c == '!': - self.state = BANG - else: - if self.state == START: - pass - elif self.state == BANG: - if c.isalpha (): - self.recv_command = c - self.recv_args = '' - self.state = CMD - else: - self.recv_error () - elif self.state == CMD: - if c == '\r': - f = Frame (self.recv_command) - f.args = binascii.unhexlify (self.recv_args) - yield f - elif (c >= '0' and c <= '9') or (c >= 'a' and c <= 'f'): - self.recv_args += c - self.state = ARG - else: - self.recv_error () - else: - assert self.state == ARG - if (c >= '0' and c <= '9') or (c >= 'a' and c <= 'f'): - self.recv_args += c - self.state = CMD - else: - self.recv_error () + """Receive a frame, used as a generator.""" + for c in self.file.read (1): + if c == '!': + self.state = BANG + else: + if self.state == START: + pass + elif self.state == BANG: + if c.isalpha (): + self.recv_command = c + self.recv_args = '' + self.state = CMD + else: + self.recv_error () + elif self.state == CMD: + if c == '\r': + f = Frame (self.recv_command) + f.args = binascii.unhexlify (self.recv_args) + yield f + elif (c >= '0' and c <= '9') or (c >= 'a' and c <= 'f'): + self.recv_args += c + self.state = ARG + else: + self.recv_error () + else: + assert self.state == ARG + if (c >= '0' and c <= '9') or (c >= 'a' and c <= 'f'): + self.recv_args += c + self.state = CMD + else: + self.recv_error () def recv_error (self): - """Handle reception errors.""" - self.state = START - if self.log: - self.log ('error') - # Resend now. - if self.send_queue: - self.send_head () + """Handle reception errors.""" + self.state = START + if self.log: + self.log ('error') + # Resend now. + if self.send_queue: + self.send_head () def dispatch (self, frame): - """Pass a received frame to the correct handler.""" - key = (frame.command, len (frame.args)) - if key in self.handlers: - h = self.handlers[key] - h[0] (*(frame.decode (h[1]))) + """Pass a received frame to the correct handler.""" + key = (frame.command, len (frame.args)) + if key in self.handlers: + h = self.handlers[key] + h[0] (*(frame.decode (h[1]))) class Frame: def __init__ (self, command = None, fmt = '', *args): - """Initiliase a frame. If command is given, the frame is constructed - using a struct.pack like fmt string.""" - if command: - assert len (command) == 1 and command.isalpha () - self.command = command - self.args = struct.pack ('!' + fmt, *args) - else: - self.command = None - self.args = '' + """Initiliase a frame. If command is given, the frame is constructed + using a struct.pack like fmt string.""" + if command: + assert len (command) == 1 and command.isalpha () + self.command = command + self.args = struct.pack ('!' + fmt, *args) + else: + self.command = None + self.args = '' def data (self): - """Get a frame representation ready to send.""" - return '!' + self.command + binascii.hexlify (self.args) + '\r' + """Get a frame representation ready to send.""" + return '!' + self.command + binascii.hexlify (self.args) + '\r' def decode (self, fmt): - """Decode using a struct.unpack like fmt string.""" - return struct.unpack ('!' + fmt, self.args) + """Decode using a struct.unpack like fmt string.""" + return struct.unpack ('!' + fmt, self.args) def __eq__ (self, other): - """Compare for equality.""" - return self.command == other.command and self.args == other.args + """Compare for equality.""" + return self.command == other.command and self.args == other.args def __str__ (self): - """Convert to string.""" - return '!' + self.command + binascii.hexlify (self.args) + """Convert to string.""" + return '!' + self.command + binascii.hexlify (self.args) diff --git a/host/proto/test/fio.py b/host/proto/test/fio.py index 07646199..f520d41e 100644 --- a/host/proto/test/fio.py +++ b/host/proto/test/fio.py @@ -1,22 +1,22 @@ class IO: def __init__ (self, fin = None, fout = None): - if fin is None: - import sys, tty - self.fin = sys.stdin - self.fout = sys.stdout - tty.setcbreak (sys.stdin.fileno ()) - else: - self.fin = fin - self.fout = fout + if fin is None: + import sys, tty + self.fin = sys.stdin + self.fout = sys.stdout + tty.setcbreak (sys.stdin.fileno ()) + else: + self.fin = fin + self.fout = fout def read (self, *args): - buf = self.fin.read (*args).replace ('\n', '\r') - return buf + buf = self.fin.read (*args).replace ('\n', '\r') + return buf def write (self, *args): - return self.fout.write (*[i.replace ('\r', '\n') for i in args]) + return self.fout.write (*[i.replace ('\r', '\n') for i in args]) def fileno (self): - return self.fin.fileno () + return self.fin.fileno () diff --git a/host/proto/test/interactive.py b/host/proto/test/interactive.py index a8251f6d..d34d6d59 100644 --- a/host/proto/test/interactive.py +++ b/host/proto/test/interactive.py @@ -26,4 +26,4 @@ while True: p.sync () fds = select.select ((p,), (), (), 0.1)[0] for i in fds: - i.read () + i.read () diff --git a/host/utils/forked.py b/host/utils/forked.py index 9e72d7d3..5185034a 100644 --- a/host/utils/forked.py +++ b/host/utils/forked.py @@ -35,14 +35,14 @@ class Forked: """ def __init__ (self, function): - """Initialise, fork, and call the given function in the children - process.""" - self.pid = os.fork () - if self.pid == 0: - function () - sys.exit (0) + """Initialise, fork, and call the given function in the children + process.""" + self.pid = os.fork () + if self.pid == 0: + function () + sys.exit (0) def kill (self): - """Kill the forked function.""" - os.kill (self.pid, signal.SIGTERM) - os.waitpid (self.pid, 0) + """Kill the forked function.""" + os.kill (self.pid, signal.SIGTERM) + os.waitpid (self.pid, 0) diff --git a/tools/dfagen/dfagen/automaton.py b/tools/dfagen/dfagen/automaton.py index 367da582..05dda350 100644 --- a/tools/dfagen/dfagen/automaton.py +++ b/tools/dfagen/dfagen/automaton.py @@ -2,110 +2,110 @@ class Event: """Event definition.""" def __init__ (self, name, comments = ''): - self.name = name - self.comments = comments - pass + self.name = name + self.comments = comments + pass def __str__ (self): - s = ' ' + self.name + '\n' - if self.comments: - s += ' ' + self.comments.replace ('\n', '\n ') + '\n' - return s + s = ' ' + self.name + '\n' + if self.comments: + s += ' ' + self.comments.replace ('\n', '\n ') + '\n' + return s class State: """State definition.""" def __init__ (self, name, comments = ''): - self.name = name - self.comments = comments - self.transitions = { } + self.name = name + self.comments = comments + self.transitions = { } def __str__ (self): - s = ' ' + self.name + '\n' - if self.comments: - s += ' ' + self.comments.replace ('\n', '\n ') + '\n' - return s + s = ' ' + self.name + '\n' + if self.comments: + s += ' ' + self.comments.replace ('\n', '\n ') + '\n' + return s def add_branch (self, branch): - if branch.event not in self.transitions: - self.transitions[branch.event] = Transition (branch.event) - self.transitions[branch.event].add_branch (branch) + if branch.event not in self.transitions: + self.transitions[branch.event] = Transition (branch.event) + self.transitions[branch.event].add_branch (branch) class Transition: """Transition definition.""" def __init__ (self, event): - self.event = event - self.branches = { } + self.event = event + self.branches = { } def add_branch (self, branch): - assert self.event is branch.event - if branch.name == None and self.branches: - raise KeyError (branch.name) - if branch.name != None and None in self.branches: - raise KeyError (branch.name) - if branch.name in self.branches: - raise KeyError (branch.name) - self.branches[branch.name] = branch + assert self.event is branch.event + if branch.name == None and self.branches: + raise KeyError (branch.name) + if branch.name != None and None in self.branches: + raise KeyError (branch.name) + if branch.name in self.branches: + raise KeyError (branch.name) + self.branches[branch.name] = branch def __str__ (self): - s = '' - for br in self.branches.values (): - s += str (br); - return s + s = '' + for br in self.branches.values (): + s += str (br); + return s class TransitionBranch: def __init__ (self, event, name = None, to = None, comments = ''): - self.event = event - self.name = name - self.to = to - self.comments = comments + self.event = event + self.name = name + self.to = to + self.comments = comments def __str__ (self): - s = ' ' + self.event.name - if self.name: - s += ': ' + self.name - s += ' -> ' + (self.to and self.to.name or '.') + '\n' - if self.comments: - s += ' ' + self.comments.replace ('\n', '\n ') + '\n' - return s + s = ' ' + self.event.name + if self.name: + s += ': ' + self.name + s += ' -> ' + (self.to and self.to.name or '.') + '\n' + if self.comments: + s += ' ' + self.comments.replace ('\n', '\n ') + '\n' + return s class Automaton: def __init__ (self, name): - self.name = name - self.comments = '' - self.initial = None - self.states = { } - self.events = { } + self.name = name + self.comments = '' + self.initial = None + self.states = { } + self.events = { } def add_state (self, state): - if state.name in self.states: - raise KeyError (state.name) - if not self.states: - self.initial = state - self.states[state.name] = state + if state.name in self.states: + raise KeyError (state.name) + if not self.states: + self.initial = state + self.states[state.name] = state def add_event (self, event): - if event.name in self.events: - raise KeyError (event.name) - self.events[event.name] = event + if event.name in self.events: + raise KeyError (event.name) + self.events[event.name] = event def __str__ (self): - s = self.name - if self.comments: - s += ' ' + self.comments.replace ('\n', '\n ') + '\n' - s += '\nStates:\n' - for state in self.states.values (): - s += str (state) - s += '\nEvents:\n' - for event in self.events.values (): - s += str (event) - s += '\n' - for state in self.states.values (): - s += state.name + ':\n' - for tr in state.transitions.values (): - s += str (tr) - s += '\n' - return s + s = self.name + if self.comments: + s += ' ' + self.comments.replace ('\n', '\n ') + '\n' + s += '\nStates:\n' + for state in self.states.values (): + s += str (state) + s += '\nEvents:\n' + for event in self.events.values (): + s += str (event) + s += '\n' + for state in self.states.values (): + s += state.name + ':\n' + for tr in state.transitions.values (): + s += str (tr) + s += '\n' + return s diff --git a/tools/dfagen/dfagen/command.py b/tools/dfagen/dfagen/command.py index 36f4803d..8a976023 100644 --- a/tools/dfagen/dfagen/command.py +++ b/tools/dfagen/dfagen/command.py @@ -10,13 +10,13 @@ from optparse import OptionParser def run (): opt = OptionParser () opt.add_option ('-d', '--dfa', dest='dfa', - help='read DFA description from FILE', metavar='FILE') + help='read DFA description from FILE', metavar='FILE') opt.add_option ('-o', '--output', dest='output', - help='choose output format', metavar='OUTPUT') + help='choose output format', metavar='OUTPUT') opt.add_option ('-c', '--config', dest='config', - help='read output configuration from FILE', metavar='FILE') + help='read output configuration from FILE', metavar='FILE') opt.add_option ('-p', '--prefix', dest='prefix', - help='use PREFIX for generated output', metavar='PREFIX') + help='use PREFIX for generated output', metavar='PREFIX') # TODO add more error checking. (options, args) = opt.parse_args () # Read automaton. diff --git a/tools/dfagen/dfagen/output/__init__.py b/tools/dfagen/dfagen/output/__init__.py index bbb8365a..567ab745 100644 --- a/tools/dfagen/dfagen/output/__init__.py +++ b/tools/dfagen/dfagen/output/__init__.py @@ -2,30 +2,30 @@ from ConfigParser import ConfigParser class UserConfig: def __init__ (self, file): - if file: - f = open (file, 'r') - cp = ConfigParser () - cp.readfp (f) - f.close () - self.dict = dict (cp.items ('user')) - if cp.has_section ('templates'): - self.templates = dict (cp.items ('templates')) - else: - self.templates = dict () + if file: + f = open (file, 'r') + cp = ConfigParser () + cp.readfp (f) + f.close () + self.dict = dict (cp.items ('user')) + if cp.has_section ('templates'): + self.templates = dict (cp.items ('templates')) + else: + self.templates = dict () def __getitem__ (self, key): - return self.dict[key] + return self.dict[key] def __contains__ (self, key): - return key in self.dict + return key in self.dict import c import dot outputs = dict ( - c = c, - dot = dot, - ) + c = c, + dot = dot, + ) def get_output (name): return outputs[name]; diff --git a/tools/dfagen/dfagen/output/c.py b/tools/dfagen/dfagen/output/c.py index 40371563..099302f9 100644 --- a/tools/dfagen/dfagen/output/c.py +++ b/tools/dfagen/dfagen/output/c.py @@ -3,162 +3,162 @@ import os.path class WriterData: def __init__ (self, prefix, automaton, user): - self.prefix = prefix - self.automaton = automaton - self.user = user - self.states = self.automaton.states.values () - self.events = self.automaton.events.values () - self.dict = dict ( - prefix = prefix, - PREFIX = prefix.upper (), - name = automaton.name, - comments = automaton.comments, - initial = automaton.initial.name, - states = self.list_states, - events = self.list_events, - states_names = self.list_states_names, - events_names = self.list_events_names, - branches = self.list_branches, - transition_table = self.transition_table, - states_template = self.states_template, - ) + self.prefix = prefix + self.automaton = automaton + self.user = user + self.states = self.automaton.states.values () + self.events = self.automaton.events.values () + self.dict = dict ( + prefix = prefix, + PREFIX = prefix.upper (), + name = automaton.name, + comments = automaton.comments, + initial = automaton.initial.name, + states = self.list_states, + events = self.list_events, + states_names = self.list_states_names, + events_names = self.list_events_names, + branches = self.list_branches, + transition_table = self.transition_table, + states_template = self.states_template, + ) def list_states (self): - return ''.join ([' ' + self.prefix.upper () + '_STATE_' + s.name - + ',\n' for s in self.states]) + return ''.join ([' ' + self.prefix.upper () + '_STATE_' + s.name + + ',\n' for s in self.states]) def list_events (self): - return ''.join ([' ' + self.prefix.upper () + '_EVENT_' - + e.name.replace (' ', '_') + ',\n' for e in self.events]) + return ''.join ([' ' + self.prefix.upper () + '_EVENT_' + + e.name.replace (' ', '_') + ',\n' for e in self.events]) def list_states_names (self): - return ''.join ([' "' + s.name + '",\n' for s in self.states]) + return ''.join ([' "' + s.name + '",\n' for s in self.states]) def list_events_names (self): - return ''.join ([' "' + e.name.replace (' ', '_') + '",\n' - for e in self.events]) + return ''.join ([' "' + e.name.replace (' ', '_') + '",\n' + for e in self.events]) def list_branches (self): - l = '' - for s in self.states: - for tr in s.transitions.values (): - for br in tr.branches.values (): - n = dict ( - PREFIX = self.prefix.upper (), - state = s.name, - event = tr.event.name.replace (' ', '_'), - branch = (br.name and br.name.replace (' ', '_') - or ''), - to = (br.to and br.to.name or s.name), - ) - l += (' %(PREFIX)s_BRANCH__%(state)s__%(event)s__%(branch)s = ' - + '_BRANCH (%(state)s, %(event)s, %(to)s),\n') % n - return l + l = '' + for s in self.states: + for tr in s.transitions.values (): + for br in tr.branches.values (): + n = dict ( + PREFIX = self.prefix.upper (), + state = s.name, + event = tr.event.name.replace (' ', '_'), + branch = (br.name and br.name.replace (' ', '_') + or ''), + to = (br.to and br.to.name or s.name), + ) + l += (' %(PREFIX)s_BRANCH__%(state)s__%(event)s__%(branch)s = ' + + '_BRANCH (%(state)s, %(event)s, %(to)s),\n') % n + return l def transition_table (self): - r = '' - for s in self.states: - r += ' { ' - es = [ ] - for e in self.events: - if e in s.transitions: - es.append (self.prefix + '__' + s.name + '__' - + e.name.replace (' ', '_')) - else: - es.append ('NULL') - r += ',\n '.join (es) - r += ' },\n' - return r + r = '' + for s in self.states: + r += ' { ' + es = [ ] + for e in self.events: + if e in s.transitions: + es.append (self.prefix + '__' + s.name + '__' + + e.name.replace (' ', '_')) + else: + es.append ('NULL') + r += ',\n '.join (es) + r += ' },\n' + return r def states_template (self, template): - t = open (os.path.join (self.templatedir, template), 'r') - tt = t.read () - t.close () - exp = '' - for s in self.states: - for tr in s.transitions.values (): - d = WriterData (self.prefix, self.automaton, self.user) - branches_to = '\n'.join ( - [(br.name and br.name or '') - + ' => ' - + (br.to and br.to.name or s.name) - + (br.comments and ('\n ' - + br.comments.replace ('\n', '\n ')) or '') - for br in tr.branches.values ()]) - returns = '\n'.join ( - [' return ' + self.prefix + '_next' - + (br.name and '_branch' or '') - + ' (' + s.name + ', ' - + tr.event.name.replace (' ', '_') - + (br.name and ', ' + br.name.replace (' ', '_') - or '') - + ');' - for br in tr.branches.values ()]) - d.dict = dict ( - prefix = self.prefix, - user = self.user, - state = s.name, - event = tr.event.name.replace (' ', '_'), - branches_to = branches_to, - returns = returns, - ) - exp += tt % d - return exp + t = open (os.path.join (self.templatedir, template), 'r') + tt = t.read () + t.close () + exp = '' + for s in self.states: + for tr in s.transitions.values (): + d = WriterData (self.prefix, self.automaton, self.user) + branches_to = '\n'.join ( + [(br.name and br.name or '') + + ' => ' + + (br.to and br.to.name or s.name) + + (br.comments and ('\n ' + + br.comments.replace ('\n', '\n ')) or '') + for br in tr.branches.values ()]) + returns = '\n'.join ( + [' return ' + self.prefix + '_next' + + (br.name and '_branch' or '') + + ' (' + s.name + ', ' + + tr.event.name.replace (' ', '_') + + (br.name and ', ' + br.name.replace (' ', '_') + or '') + + ');' + for br in tr.branches.values ()]) + d.dict = dict ( + prefix = self.prefix, + user = self.user, + state = s.name, + event = tr.event.name.replace (' ', '_'), + branches_to = branches_to, + returns = returns, + ) + exp += tt % d + return exp def __getitem__ (self, key): - preproc = lambda v: v - args = [] - key = key.split (',') - key, args = key[0], key[1:] - if key.startswith ('*'): - key = key[1:] - preproc = lambda v: ' * ' + v.replace ('\n', '\n * ') + '\n' - if key.startswith ('_'): - key = key[1:] - preproc = lambda v: v and v + '\n' or '' - val = None - if key in self.dict: - try: - val = self.dict[key] (*args) - except TypeError: - val = self.dict[key] - elif key.startswith ('user.'): - val = self.user[key[5:]] - val = preproc (val) - if val is None: - raise KeyError, key - return val + preproc = lambda v: v + args = [] + key = key.split (',') + key, args = key[0], key[1:] + if key.startswith ('*'): + key = key[1:] + preproc = lambda v: ' * ' + v.replace ('\n', '\n * ') + '\n' + if key.startswith ('_'): + key = key[1:] + preproc = lambda v: v and v + '\n' or '' + val = None + if key in self.dict: + try: + val = self.dict[key] (*args) + except TypeError: + val = self.dict[key] + elif key.startswith ('user.'): + val = self.user[key[5:]] + val = preproc (val) + if val is None: + raise KeyError, key + return val class Writer: def __init__ (self, data, templatedir): - data.templatedir = templatedir - self.data = data - self.templatedir = templatedir + data.templatedir = templatedir + self.data = data + self.templatedir = templatedir def write_template (self, template, output): - t = open (os.path.join (self.templatedir, template), 'r') - tt = t.read () - t.close () - exp = tt % self.data - o = open (output, 'w') - o.write (exp) - o.close () + t = open (os.path.join (self.templatedir, template), 'r') + tt = t.read () + t.close () + exp = tt % self.data + o = open (output, 'w') + o.write (exp) + o.close () def write (self): - templates = self.data.user.templates - if not templates: - templates = { - 'template.h': '%.h', - 'template.c': '%.c', - 'template_cb.h': '%_cb.h', - 'template_cb_skel.c': '%_cb_skel.c', - } - for (t, f) in templates.iteritems (): - self.write_template (t, f.replace ('%', self.data.prefix)) + templates = self.data.user.templates + if not templates: + templates = { + 'template.h': '%.h', + 'template.c': '%.c', + 'template_cb.h': '%_cb.h', + 'template_cb_skel.c': '%_cb_skel.c', + } + for (t, f) in templates.iteritems (): + self.write_template (t, f.replace ('%', self.data.prefix)) def write (prefix, automaton, user): w = Writer (WriterData (prefix, automaton, user), 'template-dir' in user - and user['template-dir'] or os.path.splitext (__file__)[0]) + and user['template-dir'] or os.path.splitext (__file__)[0]) w.write () diff --git a/tools/dfagen/dfagen/output/dot.py b/tools/dfagen/dfagen/output/dot.py index be346b10..d6cc057c 100644 --- a/tools/dfagen/dfagen/output/dot.py +++ b/tools/dfagen/dfagen/output/dot.py @@ -4,16 +4,16 @@ def write (prefix, automaton, user): o = open (output, 'w') o.write ('digraph %s {' % prefix) for s in automaton.states.values (): - o.write (' %s\n' % s.name) - for tr in s.transitions.values (): - for br in tr.branches.values (): - o.write (' %(state)s -> %(to)s [ label = "%(event)s" ];\n' - % dict ( - state = s.name, - event = tr.event.name - + (br.name and ': ' + br.name or ''), - to = (br.to and br.to.name or s.name), - ) - ) + o.write (' %s\n' % s.name) + for tr in s.transitions.values (): + for br in tr.branches.values (): + o.write (' %(state)s -> %(to)s [ label = "%(event)s" ];\n' + % dict ( + state = s.name, + event = tr.event.name + + (br.name and ': ' + br.name or ''), + to = (br.to and br.to.name or s.name), + ) + ) o.write ('}\n') o.close () diff --git a/tools/trace/lib/traceclass.py b/tools/trace/lib/traceclass.py index 3dd07142..4c194c73 100644 --- a/tools/trace/lib/traceclass.py +++ b/tools/trace/lib/traceclass.py @@ -4,7 +4,7 @@ class TraceParam: self.__name = name if (length == 0) or (length == 3) or (length > 4): - self.__length = 0 + self.__length = 0 raise Exception ("Length not permitted") else: self.__length = length @@ -18,12 +18,12 @@ class TraceParam: # Defines the Events of the trace module. class TraceEvent: def __init__(self, name = ''): - self.__name = name + self.__name = name self.__param_list = list() self.__string = "" def name (self): - return self.__name + return self.__name def param_add (self, name, length): param = TraceParam (name, length) -- cgit v1.2.3