From 4cd9c4458eaa49c269171145f4bc2a184948eae1 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 19 Apr 2011 21:26:15 +0200 Subject: digital/mimot: switch to differential drive --- digital/mimot/tools/inter_mimot.py | 136 +++++++++++++++++++++++++ digital/mimot/tools/mimot/init.py | 27 ++--- digital/mimot/tools/mimot/mex.py | 76 ++++---------- digital/mimot/tools/mimot/mimot.py | 201 +++++++++++++++++++++++++++++-------- 4 files changed, 331 insertions(+), 109 deletions(-) create mode 100644 digital/mimot/tools/inter_mimot.py (limited to 'digital/mimot/tools') diff --git a/digital/mimot/tools/inter_mimot.py b/digital/mimot/tools/inter_mimot.py new file mode 100644 index 00000000..219c432b --- /dev/null +++ b/digital/mimot/tools/inter_mimot.py @@ -0,0 +1,136 @@ +# inter_mimot - Mimot interface. {{{ +# +# Copyright (C) 2011 Nicolas Schodet +# +# APBTeam: +# Web: http://apbteam.org/ +# Email: team AT apbteam DOT org +# +# This program is free software; you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation; either version 2 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program; if not, write to the Free Software +# Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +# +# }}} +"""Inter, communicating with the mimot board.""" +import math + +import mimot +import mimot.init +import proto.popen_io +import serial + +from inter.inter import Inter +from Tkinter import * + +class InterMimot (Inter): + """Inter, communicating with the mimot board.""" + + def __init__ (self, argv): + # Asserv. + if argv[0] == '!': + io = proto.popen_io.PopenIO (argv[1:]) + i = mimot.init.host + else: + io = serial.Serial (argv[0]) + i = mimot.init.target + self.a = mimot.Proto (io, **i) + self.a.async = True + # Inter. + Inter.__init__ (self) + self.tk.createfilehandler (self.a, READABLE, self.read) + self.timeout () + # Query position. + self.a.register_pos () + self.a.position.register (self.pos) + + 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.backwardVar = IntVar () + self.backwardButton = Checkbutton (self.rightFrame, + text = 'backward', variable = self.backwardVar) + self.backwardButton.pack () + self.revertokVar = IntVar () + self.revertokButton = Checkbutton (self.rightFrame, + text = 'revert ok', variable = self.revertokVar) + self.revertokButton.pack () + self.ftwButton = Button (self.rightFrame, text = 'FTW', + command = self.ftw) + self.ftwButton.pack () + + self.tableview.configure (cursor = 'crosshair') + self.tableview.bind ('<1>', self.button1) + self.tableview.bind ('<3>', self.button3) + + def read (self, file, mask): + """Handle mimot events.""" + self.a.proto.read () + self.a.proto.sync () + + def timeout (self): + self.a.proto.sync () + self.after (100, self.timeout) + + def pos (self): + p = self.a.position + self.tableview.robot.pos = p.pos + self.tableview.robot.angle = p.angle + self.tableview.robot.update () + 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.backwardVar.get (), + self.revertokVar.get ()) + else: + assert 0 + + def button3 (self, ev): + x, y = self.tableview.screen_coord ((ev.x, ev.y)) + a = 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 () + + def ftw (self): + self.a.ftw (self.backwardVar.get ()) + +if __name__ == '__main__': + import sys + app = InterMimot (sys.argv[1:]) + try: + app.mainloop () + finally: + app.a.close () diff --git a/digital/mimot/tools/mimot/init.py b/digital/mimot/tools/mimot/init.py index 116cebb5..471a27bb 100644 --- a/digital/mimot/tools/mimot/init.py +++ b/digital/mimot/tools/mimot/init.py @@ -1,20 +1,21 @@ """Default parameters for asserv.""" host = dict ( - a0kp = 4, - a0a = 16, a0sm = 0x60, a0ss = 0x10, - a0be = 256, a0bs = 0x18, a0bc = 5, - a1kp = 4, - a1a = 16, a1sm = 0x60, a1ss = 0x10, - a1be = 256, a1bs = 0x18, a1bc = 5, + scale = 0.0395840674352314, f = 0xdd1, + tkp = 1, tkd = 16, + ta = 0.75, tsm = 0x20, tss = 0x10, + akp = 2, akd = 16, + aa = 0.25, asm = 0x20, ass = 0x10, E = 0x3ff, D = 0x1ff, + l = 0x1000, ) target = dict ( - a0kp = 4, - a0a = 16, a0sm = 0x60, a0ss = 0x10, - a0be = 256, a0bs = 0x18, a0bc = 5, - a1kp = 4, - a1a = 16, a1sm = 0x60, a1ss = 0x10, - a1be = 256, a1bs = 0x18, a1bc = 5, + scale = 0.0415178942124, f = 0xcef, + c = float (0x00ffbabf) / (1 << 24), + tkp = 1, tkd = 16, + ta = 0.75, tsm = 0x20, tss = 0x10, + akp = 2, akd = 16, + aa = 0.25, asm = 0x20, ass = 0x10, E = 0x3ff, D = 0x1ff, - w = 0x03, + l = 0x1000, + w = 0x00, ) diff --git a/digital/mimot/tools/mimot/mex.py b/digital/mimot/tools/mimot/mex.py index 5724189e..f39d3ebf 100644 --- a/digital/mimot/tools/mimot/mex.py +++ b/digital/mimot/tools/mimot/mex.py @@ -29,75 +29,43 @@ import simu.mex.msg class Mex: """Handle communications with simulated mimot.""" - class Aux (Observable): - """Auxiliary motor angle. + class Position (Observable): + """Robot position. + - pos: (x, y) millimeters. - angle: radian. """ - def __init__ (self): + def __init__ (self, node, instance): Observable.__init__ (self) + self.pos = None self.angle = None + node.register (instance + ':position', self.__handle) - class Pack: - """Handle reception of several Aux for one message.""" + def __handle (self, msg): + x, y, a = msg.pop ('hhl') + self.pos = (x, y) + self.angle = float (a) / 1024 + self.notify () - def __init__ (self, node, instance, list): - self.__list = list - node.register (instance + ':aux', self.__handle) + class PWM (Observable): + """Motor PWM. - def __handle (self, msg): - angles = msg.pop ('%dl' % len (self.__list)) - for aux, angle in zip (self.__list, angles): - aux.angle = float (angle) / 1024 - aux.notify () - - class Limits (Observable): - """Motor limits. - - - min, max: limits in radian. + - pwm: current PWM values (hardware unit). """ - def __init__ (self, pack, index): + def __init__ (self, node, instance): Observable.__init__ (self) - self.pack = pack - self.index = index - self.min = None - self.max = None - self.register (self.__notified) - - def __notified (self): - self.pack.set (self.index, self.min, self.max) - - class Pack: - """Handle emission of several limits for one message.""" - - def __init__ (self, node, instance): - self.node = node - self.mtype = node.reserve (instance + ':limits') - self.limits = [ None, None, None, None ] - - def set (self, index, min, max): - self.limits[index * 2] = min - self.limits[index * 2 + 1] = max - self.__send () + self.pwm = None + node.register (instance + ':pwm', self.__handle) - def __send (self): - m = simu.mex.msg.Msg (self.mtype) - for l in self.limits: - if l is None: - li = -1 - else: - li = int (l * 1024) - m.push ('l', li) - self.node.send (m) + def __handle (self, msg): + self.pwm = msg.pop ('hh') + self.notify () def __init__ (self, node, instance = 'mimot0'): - self.aux = (self.Aux (), self.Aux ()) - self.__aux_pack = self.Aux.Pack (node, instance, self.aux) - self.__limits_pack = self.Limits.Pack (node, instance) - for index, aux in enumerate (self.aux): - aux.limits = self.Limits (self.__limits_pack, index) + self.position = self.Position (node, instance) + self.pwm = self.PWM (node, instance) diff --git a/digital/mimot/tools/mimot/mimot.py b/digital/mimot/tools/mimot/mimot.py index 013a85ff..665947f5 100644 --- a/digital/mimot/tools/mimot/mimot.py +++ b/digital/mimot/tools/mimot/mimot.py @@ -44,40 +44,62 @@ class Proto: # be in the same order than in mimot program. stats_order = 'CSPW' stats_items = { - 'a0c': ('C', 0), - 'a1c': ('C', 1), - 'a0s': ('S', 0), - 'a1s': ('S', 1), - 'a0e': ('P', 0), - 'a0i': ('P', 1), - 'a1e': ('P', 2), - 'a1i': ('P', 3), - 'a0w': ('W', 0), - 'a1w': ('W', 1), + 'lc': ('C', 0), + 'rc': ('C', 1), + 'ts': ('S', 0), + 'as': ('S', 1), + 'te': ('P', 0), + 'ti': ('P', 1), + 'ae': ('P', 2), + 'ai': ('P', 3), + 'lw': ('W', 0), + 'rw': ('W', 1), } - _index = dict (a0 = 0, a1 = 1) + _index = dict (t = ord ('t'), a = ord ('a')) + + class Position (Observable): + """An observable position. To be used with register_pos. + + - pos: (x, y) millimeters. + - angle: radian. + + """ + + def __init__ (self): + Observable.__init__ (self) + self.pos = None + self.angle = None + + def handle (self, x, y, a): + """Update position and notify observers.""" + self.pos = (x, y) + self.angle = a + self.notify () def __init__ (self, file, time = time.time, **param): """Initialise communication and send parameters to asserv.""" self.proto = proto.Proto (file, time, 0.1) self.async = False - self.aseq = [ 0, 0 ] - self.aseq_ack = [ 0, 0 ] - self.proto.register ('A', 'BB', self.__handle_ack) + self.mseq = 0 + self.mseq_ack = 0 + self.proto.register ('A', 'B', 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 ( - a0kp = 0, a0ki = 0, a0kd = 0, - a0a = 1, a0sm = 0, a0ss = 0, - a0be = 2048, a0bs = 0x10, a0bc = 20, - a1kp = 0, a1ki = 0, a1kd = 0, - a1a = 1, a1sm = 0, a1ss = 0, - a1be = 2048, a1bs = 0x10, a1bc = 20, + scale = 1, + tkp = 0, tki = 0, tkd = 0, + ta = 1, tsm = 0, tss = 0, + tbe = 2048, tbs = 0x10, tbc = 20, + akp = 0, aki = 0, akd = 0, + aa = 1, asm = 0, ass = 0, + abe = 2048, abs = 0x10, abc = 20, E = 1023, I = 1023, D = 1023, + c = 1, f = 0x1000, + l = 0x2000, w = 0x00, ) self.param.update (param) @@ -135,39 +157,114 @@ class Proto: def consign (self, w, c): """Consign offset.""" - self.proto.send ('c', 'Bh', self._index[w], c) + if w == 't': + self.proto.send ('c', 'hh', c, 0) + elif w == 'a': + self.proto.send ('c', 'hh', 0, c) + else: + raise IndexError def speed (self, w, s): """Speed consign.""" - self.proto.send ('s', 'Bb', self._index[w], s) + if w == 't': + self.proto.send ('s', 'bb', s, 0) + elif w == 'a': + self.proto.send ('s', 'bb', 0, s) + else: + raise IndexError def speed_pos (self, w, offset): """Speed controlled position consign.""" - i = self._index[w] - self.aseq[i] += 1 - self.proto.send ('s', 'BlB', i, offset, self.aseq[i]) + if w == 't': + self.mseq += 1 + self.proto.send ('s', 'llB', self._dist (offset), 0, self.mseq) + elif w == 'a': + self.mseq += 1 + self.proto.send ('s', 'llB', 0, self.dist (offset), self.mseq) + else: + raise IndexError + self.wait (self.finished, auto = True) + + def speed_angle (self, w, angle): + """Speed controlled angle consign.""" + if w == 'a': + self.mseq += 1 + self.proto.send ('s', 'llB', + 0, int (round (angle * self.param['f'])), self.mseq) + else: + raise IndexError + 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', 'cl', 'X', self._dist_f248 (x)) + if y is not None: + self.proto.send ('p', 'cl', 'Y', self._dist_f248 (y)) + if a is not None: + self.proto.send ('p', 'cl', 'A', self._angle_f824 (a)) + + def goto (self, x, y, backward = False, revert_ok = False): + """Go to position.""" + self.mseq += 1 + b = 0 + if backward: + b |= 1 + if revert_ok: + b |= 2 + self.proto.send ('x', 'llBB', + self._dist_f248 (x), self._dist_f248 (y), b, self.mseq) self.wait (self.finished, auto = True) - def goto_pos (self, w, pos): - """Go to absolute position.""" - i = self._index[w] - self.aseq[i] += 1 - self.proto.send ('y', 'BhB', i, pos, self.aseq[i]) + def goto_angle (self, a): + """Go to angle.""" + self.mseq += 1 + self.proto.send ('x', 'HB', self._angle_f16 (a), self.mseq) self.wait (self.finished, auto = True) - def clamp (self, w, s, pwm): - """Clamp (speed control, then open loop PWM).""" - i = self._index[w] - self.aseq[i] += 1 - self.proto.send ('y', 'BBhB', i, s, pwm, self.aseq[i]) + def goto_xya (self, x, y, a, backward = False, revert_ok = False): + """Go to position, then angle.""" + self.mseq += 1 + b = 0 + if backward: + b |= 1 + if revert_ok: + b |= 2 + self.proto.send ('x', 'llHBB', + self._dist_f248 (x), self._dist_f248 (y), + self._angle_f16 (a), b, self.mseq) self.wait (self.finished, auto = True) + def ftw (self, backward = True): + """Go to the wall.""" + self.mseq += 1 + self.proto.send ('f', 'BB', backward and 1 or 0, self.mseq) + self.wait (self.finished, auto = True) + + def set_simu_pos (self, x, y, a): + """Set simulated position.""" + self.proto.send ('h', 'chhh', 'X', int (round (x)), int (round (y)), + int (round (a * 1024))) + + def register_pos (self, func = None, interval = 225 / 4): + """Will call func each time a position is received. If no function is + provided, use the Position observable object.""" + if func is None: + self.position = self.Position () + self.pos_func = self.position.handle + else: + self.pos_func = func + self.proto.register ('X', 'lll', self.__handle_pos) + self.proto.send ('X', 'B', interval) + def send_param (self): """Send all parameters.""" p = self.param def f88 (x): return int (round (x * (1 << 8))) - for m in ('a0', 'a1'): + def f824 (x): + return int (round (x * (1 << 24))) + for m in ('t', 'a'): index = self._index [m] self.proto.send ('p', 'cBH', 'p', index, f88 (p[m + 'kp'])) self.proto.send ('p', 'cBH', 'i', index, f88 (p[m + 'ki'])) @@ -179,6 +276,9 @@ class Proto: self.proto.send ('p', 'cH', 'E', p['E']) self.proto.send ('p', 'cH', 'I', p['I']) self.proto.send ('p', 'cH', 'D', p['D']) + self.proto.send ('p', 'cL', 'c', f824 (p['c'])) + self.proto.send ('p', 'cH', 'f', p['f']) + self.proto.send ('p', 'cH', 'l', p['l']) self.proto.send ('p', 'cB', 'w', p['w']) def write_eeprom (self): @@ -196,11 +296,17 @@ class Proto: self.stats_line = [ ] self.stats_count += 1 - def __handle_ack (self, a0seq, a1seq): + def __handle_ack (self, mseq): """Record current acknowledge level and acknowledge reception.""" - self.aseq_ack[0] = a0seq & 0x7f - self.aseq_ack[1] = a1seq & 0x7f - self.proto.send ('a', 'BB', a0seq, a1seq) + self.mseq_ack = mseq & 0x7f + self.proto.send ('a', 'B', mseq) + + def __handle_pos (self, x, y, a): + """Handle position report.""" + x = x / 256 * self.param['scale'] + y = y / 256 * self.param['scale'] + a = a * 2 * math.pi / (1 << 24) + self.pos_func (x, y, a) def wait (self, cond = None, auto = False): """Wait for a condition to become true, or for a number of recorded @@ -216,8 +322,7 @@ class Proto: def finished (self): """Return True if movement commands have been acknowledged.""" - return (self.aseq[0] == self.aseq_ack[0] - and self.aseq[1] == self.aseq_ack[1]) + return self.mseq == self.mseq_ack def free (self): """Coast motors.""" @@ -239,3 +344,15 @@ class Proto: """Return fileno for select() calls.""" return self.proto.fileno () + def _dist (self, d): + return int (round (d / self.param['scale'])) + + def _dist_f248 (self, d): + return int (round ((1 << 8) * d / self.param['scale'])) + + def _angle_f16 (self, a): + return int (round ((1 << 16) * a / (2 * math.pi))) & 0xffff + + def _angle_f824 (self, a): + return int (round ((1 << 24) * a / (2 * math.pi))) + -- cgit v1.2.3