summaryrefslogtreecommitdiff
path: root/digital/mimot/tools
diff options
context:
space:
mode:
authorNicolas Schodet2011-04-19 21:26:15 +0200
committerNicolas Schodet2011-04-19 21:26:15 +0200
commit4cd9c4458eaa49c269171145f4bc2a184948eae1 (patch)
tree780cdc9493bfcfd2907a61cce62ff8a6f64a0c45 /digital/mimot/tools
parente2594ccdadae65d7e7473e51fb8bfbbd49c04a24 (diff)
digital/mimot: switch to differential drive
Diffstat (limited to 'digital/mimot/tools')
-rw-r--r--digital/mimot/tools/inter_mimot.py136
-rw-r--r--digital/mimot/tools/mimot/init.py27
-rw-r--r--digital/mimot/tools/mimot/mex.py76
-rw-r--r--digital/mimot/tools/mimot/mimot.py201
4 files changed, 331 insertions, 109 deletions
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)))
+