From 78aa0c38b86c7cf124115e3e43a8e79ade36b2a4 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Wed, 13 May 2009 18:55:47 +0200 Subject: * host/simu: - take puck on table. --- host/simu/robots/aquajim/model/bag.py | 2 +- host/simu/robots/aquajim/model/sorter.py | 28 +++++++++++++++++++++++++++- host/simu/robots/aquajim/view/arm.py | 32 +++++++++++++++++++++----------- 3 files changed, 49 insertions(+), 13 deletions(-) (limited to 'host/simu/robots/aquajim') diff --git a/host/simu/robots/aquajim/model/bag.py b/host/simu/robots/aquajim/model/bag.py index ace646b6..31827c35 100644 --- a/host/simu/robots/aquajim/model/bag.py +++ b/host/simu/robots/aquajim/model/bag.py @@ -39,7 +39,7 @@ class Bag: 2 * pi, 0, pi / 2) self.sorter = Sorter (table, link_bag.asserv.aux[0], link_bag.asserv.aux[1], link_bag.io.servo[0:2], - self.elevator_door) + self.elevator_door, (self.position, )) self.distance_sensor = [ DistanceSensorSharps (link_bag.io.adc[0], scheduler, table, (50, 130), 0, (self.position, )), diff --git a/host/simu/robots/aquajim/model/sorter.py b/host/simu/robots/aquajim/model/sorter.py index 5c4a013f..99615b7c 100644 --- a/host/simu/robots/aquajim/model/sorter.py +++ b/host/simu/robots/aquajim/model/sorter.py @@ -23,14 +23,16 @@ # }}} """AquaJim pucks sorter.""" from utils.observable import Observable +from simu.utils.trans_matrix import TransMatrix from math import pi class Sorter (Observable): def __init__ (self, table, arm_motor_link, elevator_motor_link, - servo_links, elevator_door_model): + servo_links, elevator_door_model, into = None): Observable.__init__ (self) self.table = table + self.into = into or () self.arm_motor_link = arm_motor_link self.arm_motor_link.register (self.__arm_motor_notified) self.__arm_motor_notified () @@ -39,9 +41,33 @@ class Sorter (Observable): self.__elevator_motor_notified () self.servo_links = servo_links self.elevator_door = elevator_door_model + self.arm_slot = [ None, None, None ] + + def __transform (self, pos): + m = TransMatrix () + for i in self.into: + if i.pos is None: + return None + m.rotate (i.angle) + m.translate (i.pos) + return m.apply (pos) def __arm_motor_notified (self): + # Update angle. self.arm_angle = self.arm_motor_link.angle + if self.arm_angle is not None: + # Is there a puck at entry? + entry_pos = self.__transform ((150 - 40, 0)) + if entry_pos is not None: + front_puck = self.table.nearest (entry_pos, level = 1, max = 35) + # For each arm. + for i in range (3): + a = (self.arm_angle + i * 2 * pi / 3) % (2 * pi) + # If arm is leaving, take puck. + if (front_puck is not None and self.arm_slot[i] is None + and a > 0 and a < pi / 16): + self.arm_slot[i] = front_puck + front_puck.pos = None self.notify () def __elevator_motor_notified (self): diff --git a/host/simu/robots/aquajim/view/arm.py b/host/simu/robots/aquajim/view/arm.py index ee8aaa95..aab69c58 100644 --- a/host/simu/robots/aquajim/view/arm.py +++ b/host/simu/robots/aquajim/view/arm.py @@ -25,10 +25,12 @@ from simu.inter.drawable import Drawable from math import pi, cos, sin +from simu.view.table_eurobot2009 import puck_attr + class Arm (Drawable): - width = 1 - height = 1 + width = 320 + height = 320 def __init__ (self, onto, model): Drawable.__init__ (self, onto) @@ -42,18 +44,26 @@ class Arm (Drawable): def draw (self): self.reset () - self.draw_arc ((0, 0), 0.45, start = 7 * pi / 12, extent = 10 * pi / 12, + self.draw_arc ((0, 0), 150, start = 7 * pi / 12, extent = 10 * pi / 12, style = 'arc', outline = '#808080') - self.draw_arc ((0, 0), 0.45, start = -5 * pi / 12, extent = 10 * pi / 12, + self.draw_arc ((0, 0), 150, start = -5 * pi / 12, extent = 10 * pi / 12, style = 'arc', outline = '#808080') - self.draw_arc ((0, 0), 0.25, start = -7 * pi / 12, extent = 14 * pi / 12, + self.draw_arc ((0, 0), 100, start = -7 * pi / 12, extent = 14 * pi / 12, style = 'arc', outline = '#808080') if self.angle is not None: - self.trans_scale (0.4) self.trans_rotate (-self.angle) - self.draw_line ((0, 0), (0, 1)) - self.draw_line ((0, 1), (0.3, 1), arrow = 'last', fill = '#808080') - self.draw_line ((0, 0), (cos (pi / 6), -sin (pi / 6))) - self.draw_line ((0, 0), (-cos (pi / 6), -sin (pi / 6))) - Drawable.draw (self) + for i in range (3): + self.draw_line ((0, 0), (0, 135)) + self.draw_arc ((0, 0), 135, start = pi / 2, extent = pi / 3, + style = 'arc') + if i == 0: + self.draw_line ((0, 135), (70, 135), arrow = 'last', + fill = '#808080') + a = -2 * pi / 3 + self.trans_rotate (0.15 * a) + puck = self.model.arm_slot[i] + if puck is not None: + self.draw_rectangle ((-35, 140), (35, 110), + **puck_attr[puck.color]) + self.trans_rotate ((1 - 0.15) * a) -- cgit v1.2.3