From 297f37560c89fa66a0341d3d87617801d2fbf39d Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 3 Mar 2009 00:59:20 +0100 Subject: * host/inter: - use io mex interface. --- host/inter/dist_sensor.py | 8 +++--- host/inter/inter.py | 23 +++++++++-------- host/inter/inter_node.py | 66 +++++++++++++++++++++++------------------------ 3 files changed, 49 insertions(+), 48 deletions(-) (limited to 'host/inter') diff --git a/host/inter/dist_sensor.py b/host/inter/dist_sensor.py index b1bd0d0d..859b311f 100644 --- a/host/inter/dist_sensor.py +++ b/host/inter/dist_sensor.py @@ -22,17 +22,18 @@ # # }}} """Distance sensor.""" +from math import pi, cos, sin, sqrt from Tkinter import * from drawable import * +from utils.observable import Observable -from math import pi, cos, sin, sqrt - -class DistSensor (Drawable): +class DistSensor (Drawable, Observable): """A distance sensor.""" def __init__ (self, onto, pos, angle, range): Drawable.__init__ (self, onto) + Observable.__init__ (self) self.pos = pos self.angle = angle self.range = range @@ -66,6 +67,7 @@ class DistSensor (Drawable): d = m + f if self.distance is None or self.distance > d: self.distance = d + self.notify () return self.distance def draw (self): diff --git a/host/inter/inter.py b/host/inter/inter.py index 4c19e5c8..8dedd680 100644 --- a/host/inter/inter.py +++ b/host/inter/inter.py @@ -177,18 +177,19 @@ class Arm (Drawable): class Servo: """Servo motor.""" - def __init__ (self, coord, l, start, extent): + def __init__ (self, onto, coord, l, start, extent): + self.onto = onto 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, + def draw (self): + self.onto.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.onto.draw_line (self.coord, (self.coord[0] + self.l * cos (a), self.coord[1] + self.l * sin (a))) @@ -198,19 +199,19 @@ class Rear (Drawable): 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), + Servo (self, (-2.5, -1), 0.8, 0, pi/2), + Servo (self, (-1.5, -0.9), 0.8, 0, pi/2), + Servo (self, (-0.5, -0.8), 0.8, 0, pi/2), + Servo (self, (0.5, -0.8), 0.8, pi, -pi/2), + Servo (self, (1.5, -0.9), 0.8, pi, -pi/2), + Servo (self, (-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) + i.draw () 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), diff --git a/host/inter/inter_node.py b/host/inter/inter_node.py index d371920d..8b0c9df9 100644 --- a/host/inter/inter_node.py +++ b/host/inter/inter_node.py @@ -33,6 +33,7 @@ from mex.node import Node from mex.msg import Msg import asserv +import io class InterNode (Inter): """Inter, coupled with a mex Node.""" @@ -41,23 +42,22 @@ class InterNode (Inter): # period. TICK = 900.0 - IO_JACK = 0xb0 - IO_COLOR = 0xb1 - IO_SERVO = 0xb2 - IO_SHARPS = 0xb3 - IO_PATH = 0xb4 - def __init__ (self): Inter.__init__ (self) self.node = Node () self.asserv_link = asserv.Mex (self.node) self.asserv_link.position.register (self.notify_position) self.asserv_link.aux[0].register (self.notify_aux0) - 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.io_link = io.Mex (self.node) + self.notify_jack () + self.notify_color_switch () + self.jackVar.trace_variable ('w', + lambda *args: self.notify_jack ()) + self.colorVar.trace_variable ('w', + lambda *args: self.notify_color_switch ()) + for i in range (len (self.io_link.servo)): + self.io_link.servo[i].register ( + lambda i = i: self.notify_servo (i)) self.tk.createfilehandler (self.node, READABLE, self.read) self.date = 0 self.synced = True @@ -74,10 +74,12 @@ class InterNode (Inter): for s in self.dist_sensors: s.obstacles = self.obstacles s.hide = True + s.register (self.update_sharps) self.tableview.robot.drawn.extend (self.dist_sensors) + self.update_sharps () self.path = Path (self.tableview.table) self.tableview.drawn.append (self.path) - self.tableview + self.io_link.path.register (self.notify_path) def createWidgets (self): Inter.createWidgets (self) @@ -149,39 +151,35 @@ class InterNode (Inter): self.actuatorview.arm.angle = self.asserv_link.aux[0].angle 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) + def notify_jack (self): + self.io_link.jack.state = self.jackVar.get () + self.io_link.jack.notify () - def handle_IO_COLOR (self, msg): - m = Msg (self.IO_COLOR) - m.push ('B', self.colorVar.get ()) - self.node.response (m) + def notify_color_switch (self): + self.io_link.color_switch.state = self.colorVar.get () + self.io_link.color_switch.notify () - 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) + def notify_servo (self, i): + servo = self.io_link.servo[i] + trap = self.actuatorview.rear.traps[i] + trap.pos = float (servo.value) / 255 + self.update (trap) - def handle_IO_SHARPS (self, msg): - m = Msg (self.IO_SHARPS) - for i in self.dist_sensors: - d = i.distance or 800 + def update_sharps (self): + for ds, adc in zip (self.dist_sensors, self.io_link.adc): + d = ds.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) + adc.value = v + adc.notify () - def handle_IO_PATH (self, msg): - self.path.path = [ ] - while len (msg) >= 4: - self.path.path.append (msg.pop ('hh')) + def notify_path (self): + self.path.path = self.io_link.path.path self.update (self.path) def place_obstacle (self, ev): -- cgit v1.2.3