def c2 ():
n = Node ()
mtype_oucouc = n.reserve ('oucouc')
mtype_coucou = n.reserve ('coucou')
def a (msg):
print 'coucou'
n.register (mtype_coucou, a)
m = Msg (mtype_oucouc)
m.push ('B', 42)
r = n.request (m)
assert r.mtype == mtype_oucouc
assert r.pop ('B') == (43,)
n.wait_async (42)
while not n.sync ():
fds = select.select ((n, ), (), ())[0]
for i in fds:
i.read ()
n.wait ()
class InterNode (Inter):
def __init__ (self, tick):
Inter.__init__ (self)
# Create node and bind to Tk.
self.node = Node ()
self.node.tick = tick # tick/s
self.tk.createfilehandler (self.node, READABLE, self.read)
# Animation attributes.
self.date = 0
self.synced = True
self.step_after = None
self.step_time = None
def create_widgets (self):
Inter.create_widgets (self)
self.now_label = Label (self.right_frame, text = 'Now: 0 s')
self.now_label.pack ()
self.step_button = Button (self.right_frame, text = 'Step',
command = self.step)
self.step_button.pack ()
self.step_size_scale = Scale (self.right_frame, orient = HORIZONTAL,
from_ = 0.05, to = 1.0, resolution = 0.05)
self.step_size_scale.pack ()
self.play_var = IntVar ()
self.play_button = Checkbutton (self.right_frame,
variable = self.play_var, text = 'Play', command = self.play,
underline = 0)
self.play_button.pack ()
self.bind_all ('p', lambda event: self.play_button.invoke ())
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.step_size_scale.get () * self.node.tick))
self.synced = False
self.step_after = None
self.step_time = time.time ()
def play (self):
"""Activate auto-steping."""
if self.play_var.get ():
if self.step_after is None and self.synced:
self.step ()
self.step_button.configure (state = DISABLED)
else:
if self.step_after is not None:
self.after_cancel (self.step_after)
self.step_after = None
self.step_button.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.now_label.configure (text = 'Now: %.2f s'
% (float (self.date) / self.node.tick))
self.update ()
if self.play_var.get ():
assert self.step_after is None
next = self.step_time + self.step_size_scale.get ()
delay = next - time.time ()
if delay > 0:
self.step_after = self.after (int (delay * 1000),
self.step)
else:
self.step_after = self.after_idle (self.step)
class InterNode (Inter):
"""Inter, coupled with a mex Node."""
# There is 900 tick per seconds, to permit AVR to have a 4.44444 ms
# period.
TICK = 900.0
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.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
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
s.register (self.update_sharps)
self.path = Path (self.tableview.table)
self.io_link.path.register (self.notify_path)
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 ()
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 ()
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)
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 ()
def notify_position (self):
self.tableview.robot.pos = self.asserv_link.position.pos
self.tableview.robot.angle = self.asserv_link.position.angle
self.tableview.robot.update ()
def notify_aux0 (self):
self.actuatorview.arm.angle = self.asserv_link.aux[0].angle
self.actuatorview.arm.update ()
def notify_jack (self):
self.io_link.jack.state = self.jackVar.get ()
self.io_link.jack.notify ()
def notify_color_switch (self):
self.io_link.color_switch.state = self.colorVar.get ()
self.io_link.color_switch.notify ()
def notify_servo (self, i):
servo = self.io_link.servo[i]
trap = self.actuatorview.rear.traps[i]
trap.pos = servo.value
trap.update ()
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 = int (round (v * 1024 / 5))
assert v >= 0 and v < 1024
adc.value = v
adc.notify ()
def notify_path (self):
self.path.path = self.io_link.path.path
self.path.update ()
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))
for d in self.obstacles + self.dist_sensors:
d.update ()
self.update ()
def show_sensors (self):
hide = not self.showSensorsVar.get ()
for i in self.dist_sensors:
i.hide = hide
i.update ()
self.update ()