summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--digital/asserv/tools/asserv/__init__.py3
-rw-r--r--digital/asserv/tools/asserv/asserv.py66
-rw-r--r--digital/asserv/tools/asserv/mex.py100
-rw-r--r--digital/asserv/tools/inter_asserv.py4
-rw-r--r--digital/asserv/tools/step.py7
-rw-r--r--digital/asserv/tools/test_goto.py8
-rw-r--r--digital/asserv/tools/write_eeprom.py4
-rw-r--r--digital/io/tools/test_simu.py5
8 files changed, 172 insertions, 25 deletions
diff --git a/digital/asserv/tools/asserv/__init__.py b/digital/asserv/tools/asserv/__init__.py
index a498dc6b..7a8b5d6f 100644
--- a/digital/asserv/tools/asserv/__init__.py
+++ b/digital/asserv/tools/asserv/__init__.py
@@ -1 +1,2 @@
-from asserv import Asserv
+from asserv import Proto
+from mex import Mex
diff --git a/digital/asserv/tools/asserv/asserv.py b/digital/asserv/tools/asserv/asserv.py
index 31cfce2d..7109e7b3 100644
--- a/digital/asserv/tools/asserv/asserv.py
+++ b/digital/asserv/tools/asserv/asserv.py
@@ -21,13 +21,18 @@
# Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
#
# }}} */
+"""Proto interface to asserv."""
+
import time
import numpy
import math
import proto
+from utils.observable import Observable
-class Asserv:
+class Proto:
+ """Provide functions to communicate with asserv using the proto
+ interface."""
stats_format = {
'C': 'HHH',
@@ -58,16 +63,36 @@ class Asserv:
'a0w': ('W', 2),
}
+ 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.mseq = 0
self.mseq_ack = 0
self.a0seq = 0
self.a0seq_ack = 0
- self.proto.register ('A', 'BB', self.handle_ack)
+ self.proto.register ('A', 'BB', self.__handle_ack)
def make_handle (s):
- return lambda *args: self.handle_stats (s, *args)
+ 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
@@ -87,7 +112,8 @@ class Asserv:
self.send_param ()
def stats (self, *stats_items, **options):
- """Activate stats."""
+ """Activate stats. Take a list of items to record, and an optional
+ interval option."""
interval = 1
if 'interval' in options:
interval = options['interval']
@@ -115,6 +141,7 @@ class Asserv:
self.stats_line = [ ]
def get_stats (self, wait = None):
+ """Get recorded stats. Return an array with every requested stats."""
if wait:
self.wait (wait)
list = self.stats_list
@@ -207,13 +234,19 @@ class Asserv:
"""Set simulated position."""
self.proto.send ('h', 'BHHH', ord ('X'), x, y, 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)
+ 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
self.proto.send ('p', 'BHH', ord ('p'), p['tkp'] * 256,
p['akp'] * 256)
@@ -239,11 +272,13 @@ class Asserv:
self.proto.send ('p', 'BH', ord ('l'), p['l'])
def write_eeprom (self):
+ """Request an EEPROM write."""
self.proto.send ('p', 'BB', ord ('E'), 1)
time.sleep (1)
self.wait (lambda: True)
- def handle_stats (self, stat, *args):
+ def __handle_stats (self, stat, *args):
+ """Record received stats."""
if self.stats_enabled is not None:
self.stats_line.extend (args)
if self.stats_counter == stat:
@@ -251,18 +286,22 @@ class Asserv:
self.stats_line = [ ]
self.stats_count += 1
- def handle_ack (self, mseq, a0seq):
+ def __handle_ack (self, mseq, a0seq):
+ """Record current acknowledge level and acknowledge reception."""
self.mseq_ack = mseq & 0x7f
self.a0seq_ack = a0seq & 0x7f
self.proto.send ('a', 'BB', mseq, a0seq)
- def handle_pos (self, x, y, a):
+ 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
+ statistics. If auto is True, do not wait in asynchronous mode."""
if auto and self.async:
return
try:
@@ -273,21 +312,26 @@ class Asserv:
self.proto.wait (cond)
def finished (self):
+ """Return True if movement commands have been acknowledged."""
return self.mseq == self.mseq_ack and self.a0seq == self.a0seq_ack
def free (self):
+ """Coast motors."""
self.proto.send ('w')
def reset (self):
+ """Coast all motors and reset asserv."""
self.proto.send ('w')
self.proto.send ('w', 'H', 0)
self.proto.send ('z')
self.proto.send ('z')
def close (self):
+ """Gracefully close communications."""
self.reset ()
self.wait (lambda: True)
self.proto.file.close ()
def fileno (self):
+ """Return fileno for select() calls."""
return self.proto.fileno ()
diff --git a/digital/asserv/tools/asserv/mex.py b/digital/asserv/tools/asserv/mex.py
new file mode 100644
index 00000000..c673e8e0
--- /dev/null
+++ b/digital/asserv/tools/asserv/mex.py
@@ -0,0 +1,100 @@
+# asserv - Position & speed motor control on AVR. {{{
+#
+# Copyright (C) 2009 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.
+#
+# }}} */
+"""Mex interface to asserv."""
+
+from utils.observable import Observable
+
+ID_POSITION = 0xa0
+ID_PWM = 0xa1
+ID_AUX = 0xa8
+
+class Mex:
+ """Handle communications with simulated asserv."""
+
+ class Position (Observable):
+ """Robot position.
+
+ - pos: (x, y) millimeters.
+ - angle: radian.
+
+ """
+
+ def __init__ (self, node):
+ Observable.__init__ (self)
+ self.pos = None
+ self.angle = None
+ node.register (ID_POSITION, self.__handle)
+
+ def __handle (self, msg):
+ x, y, a = msg.pop ('hhl')
+ self.pos = (x, y)
+ self.angle = float (a) / 1024
+ self.notify ()
+
+ class PWM (Observable):
+ """Motor PWM.
+
+ - pwm: current PWM value (hardware unit).
+
+ """
+
+ def __init__ (self, node):
+ Observable.__init__ (self)
+ self.pwm = None
+ node.register (ID_PWM, self.__handle)
+
+ def __handle (self, msg):
+ self.pwm = msg.pop ('hhh')
+ self.notify ()
+
+ class Aux (Observable):
+ """Auxiliary motor angle.
+
+ - angle: radian.
+
+ """
+
+ def __init__ (self):
+ Observable.__init__ (self)
+ self.angle = None
+
+ class Pack:
+ """Handle reception of several Aux for one message."""
+
+ def __init__ (self, node, list):
+ self.__list = list
+ node.register (ID_AUX, self.__handle)
+
+ 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 ()
+
+ def __init__ (self, node):
+ self.position = self.Position (node)
+ self.pwm = self.PWM (node)
+ self.aux = (self.Aux (), )
+ self.__aux_pack = self.Aux.Pack (node, self.aux)
+
diff --git a/digital/asserv/tools/inter_asserv.py b/digital/asserv/tools/inter_asserv.py
index ad7edf0e..8c2fbf34 100644
--- a/digital/asserv/tools/inter_asserv.py
+++ b/digital/asserv/tools/inter_asserv.py
@@ -24,7 +24,7 @@
"""Inter, communicating with the asserv board."""
import math
-from asserv import Asserv
+import asserv
import asserv.init
import proto.popen_io
import serial
@@ -43,7 +43,7 @@ class InterAsserv (Inter):
else:
io = serial.Serial (argv[0])
i = asserv.init.target
- self.a = Asserv (io, **i)
+ self.a = asserv.Proto (io, **i)
self.a.async = True
self.a.register_pos (self.pos)
# Inter.
diff --git a/digital/asserv/tools/step.py b/digital/asserv/tools/step.py
index 04a50961..5b5040b1 100644
--- a/digital/asserv/tools/step.py
+++ b/digital/asserv/tools/step.py
@@ -1,9 +1,10 @@
import sys
-from asserv import Asserv
+import Gnuplot
+
+import asserv
import proto.popen_io
import serial
-import Gnuplot
def step (name, offset, kp, ki, kd, plots, **param):
if sys.argv[1] == '!':
@@ -12,7 +13,7 @@ def step (name, offset, kp, ki, kd, plots, **param):
io = serial.Serial (sys.argv[1])
p = { name + 'kp': kp, name + 'ki': ki, name + 'kd': kd}
p.update (param)
- a = Asserv (io, **p)
+ a = asserv.Proto (io, **p)
a.stats (*plots)
a.consign (name, offset)
#a.speed (name, 16)
diff --git a/digital/asserv/tools/test_goto.py b/digital/asserv/tools/test_goto.py
index 050779a9..da9ee41c 100644
--- a/digital/asserv/tools/test_goto.py
+++ b/digital/asserv/tools/test_goto.py
@@ -1,11 +1,11 @@
import sys
+import random
+import math
-from asserv import Asserv
+import asserv
import asserv.init
import proto.popen_io
import serial
-import random
-import math
if sys.argv[1] == '!':
io = proto.popen_io.PopenIO (sys.argv[2:])
@@ -13,7 +13,7 @@ if sys.argv[1] == '!':
else:
io = serial.Serial (sys.argv[1])
init = asserv.init.target
-a = Asserv (io, **init)
+a = asserv.Proto (io, **init)
for i in xrange (10):
x = random.randrange (2000)
y = random.randrange (1100)
diff --git a/digital/asserv/tools/write_eeprom.py b/digital/asserv/tools/write_eeprom.py
index c66e3f25..b6b9abd9 100644
--- a/digital/asserv/tools/write_eeprom.py
+++ b/digital/asserv/tools/write_eeprom.py
@@ -1,6 +1,6 @@
import sys
-from asserv import Asserv
+import asserv
import asserv.init
import proto.popen_io
import serial
@@ -11,6 +11,6 @@ if sys.argv[1] == '!':
else:
io = serial.Serial (sys.argv[1])
init = asserv.init.target
-a = Asserv (io, **init)
+a = asserv.Proto (io, **init)
a.write_eeprom ()
a.close ()
diff --git a/digital/io/tools/test_simu.py b/digital/io/tools/test_simu.py
index 81ef0eb3..53a39ff3 100644
--- a/digital/io/tools/test_simu.py
+++ b/digital/io/tools/test_simu.py
@@ -26,7 +26,7 @@ import math
import mex.hub
import utils.forked
-from asserv import Asserv
+import asserv
import asserv.init
from io import Io
import io.init
@@ -50,7 +50,8 @@ class TestSimu (InterNode):
def time ():
return self.node.date / self.TICK
# Asserv.
- self.asserv = Asserv (PopenIO (asserv_cmd), time, **asserv.init.host)
+ self.asserv = asserv.Proto (PopenIO (asserv_cmd), time,
+ **asserv.init.host)
self.asserv.async = True
self.tk.createfilehandler (self.asserv, READABLE, self.asserv_read)
# Io.