summaryrefslogtreecommitdiff
path: root/host/simu
diff options
context:
space:
mode:
Diffstat (limited to 'host/simu')
-rw-r--r--host/simu/model/table.py12
-rw-r--r--host/simu/robots/aquajim/model/bag.py2
-rw-r--r--host/simu/robots/aquajim/model/sorter.py28
-rw-r--r--host/simu/robots/aquajim/view/arm.py32
4 files changed, 61 insertions, 13 deletions
diff --git a/host/simu/model/table.py b/host/simu/model/table.py
index 90cff707..41767ac4 100644
--- a/host/simu/model/table.py
+++ b/host/simu/model/table.py
@@ -45,3 +45,15 @@ class Table:
return i
return i
+ def nearest (self, pos, level = None, max = None):
+ """Return nearest object."""
+ no = None
+ nds = None
+ for o in self.obstacles:
+ if o.pos is not None and (level is None or level == o.level):
+ ds = (pos[0] - o.pos[0]) ** 2 + (pos[1] - o.pos[1]) ** 2
+ if (max is None or ds < max ** 2) and (nds is None or ds < nds):
+ no = o
+ nds = ds
+ return no
+
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)