summaryrefslogtreecommitdiff
path: root/host
diff options
context:
space:
mode:
Diffstat (limited to 'host')
-rw-r--r--host/simu/robots/aquajim/model/sorter.py39
-rw-r--r--host/simu/robots/aquajim/view/bag.py3
-rw-r--r--host/simu/robots/aquajim/view/bridge.py59
3 files changed, 100 insertions, 1 deletions
diff --git a/host/simu/robots/aquajim/model/sorter.py b/host/simu/robots/aquajim/model/sorter.py
index 99615b7c..d5452aa5 100644
--- a/host/simu/robots/aquajim/model/sorter.py
+++ b/host/simu/robots/aquajim/model/sorter.py
@@ -33,6 +33,9 @@ class Sorter (Observable):
Observable.__init__ (self)
self.table = table
self.into = into or ()
+ self.arm_slot = [ None, None, None ]
+ self.bridge_slot = [ None, None ]
+ self.lost = [ ]
self.arm_motor_link = arm_motor_link
self.arm_motor_link.register (self.__arm_motor_notified)
self.__arm_motor_notified ()
@@ -40,8 +43,11 @@ class Sorter (Observable):
self.elevator_motor_link.register (self.__elevator_motor_notified)
self.__elevator_motor_notified ()
self.servo_links = servo_links
+ self.servo_links[0].register (self.__bridge_door_servo_notified)
+ self.__bridge_door_servo_notified ()
+ self.servo_links[1].register (self.__bridge_finger_servo_notified)
+ self.__bridge_finger_servo_notified ()
self.elevator_door = elevator_door_model
- self.arm_slot = [ None, None, None ]
def __transform (self, pos):
m = TransMatrix ()
@@ -68,6 +74,14 @@ class Sorter (Observable):
and a > 0 and a < pi / 16):
self.arm_slot[i] = front_puck
front_puck.pos = None
+ # If arm is up, pass puck to bridge.
+ if (self.arm_slot[i] is not None
+ and a > pi and a < 17 * pi / 16):
+ if self.bridge_slot[0] is None:
+ self.bridge_slot[0] = self.arm_slot[i]
+ else:
+ self.lost.append (self.arm_slot[i])
+ self.arm_slot[i] = None
self.notify ()
def __elevator_motor_notified (self):
@@ -77,3 +91,26 @@ class Sorter (Observable):
self.elevator_height = 150 - self.elevator_motor_link.angle * 5.5
self.notify ()
+ def __bridge_door_servo_notified (self):
+ self.bridge_door_servo_value = self.servo_links[0].value
+ if self.bridge_door_servo_value is not None:
+ if (self.bridge_door_servo_value < 0.1
+ and self.bridge_slot[0] is not None
+ and self.bridge_slot[1] is None):
+ # Pass the door.
+ self.bridge_slot[1] = self.bridge_slot[0]
+ self.bridge_slot[0] = None
+ self.notify ()
+
+ def __bridge_finger_servo_notified (self):
+ self.bridge_finger_servo_value = self.servo_links[1].value
+ if (self.bridge_finger_servo_value is not None
+ and self.bridge_door_servo_value is not None):
+ if (self.bridge_door_servo_value > 0.9
+ and self.bridge_finger_servo_value > 0.5
+ and self.bridge_slot[1] is not None):
+ # Drop until elevator is fully implemented.
+ self.lost.append (self.bridge_slot[1])
+ self.bridge_slot[1] = None
+ self.notify ()
+
diff --git a/host/simu/robots/aquajim/view/bag.py b/host/simu/robots/aquajim/view/bag.py
index c617f381..c3375946 100644
--- a/host/simu/robots/aquajim/view/bag.py
+++ b/host/simu/robots/aquajim/view/bag.py
@@ -27,6 +27,7 @@ from simu.view.distance_sensor import DistanceSensor
from simu.view.path import Path
from simu.robots.aquajim.view.robot import Robot
from simu.robots.aquajim.view.arm import Arm
+from simu.robots.aquajim.view.bridge import Bridge
from simu.robots.aquajim.view.elevator import Elevator
class Bag:
@@ -38,6 +39,8 @@ class Bag:
self.robot = Robot (table, model_bag.position)
self.arm = Arm (actuator_view.add_view (Arm.width, Arm.height),
model_bag.sorter)
+ self.bridge = Bridge (actuator_view.add_view (Bridge.width,
+ Bridge.height), model_bag.sorter)
self.elevator = Elevator (actuator_view.add_view (Elevator.width,
Elevator.height), model_bag.sorter)
self.distance_sensor = [DistanceSensor (self.robot, ds)
diff --git a/host/simu/robots/aquajim/view/bridge.py b/host/simu/robots/aquajim/view/bridge.py
new file mode 100644
index 00000000..59c86211
--- /dev/null
+++ b/host/simu/robots/aquajim/view/bridge.py
@@ -0,0 +1,59 @@
+# simu - Robot simulation. {{{
+#
+# 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.
+#
+# }}}
+"""AquaJim bridge."""
+from simu.inter.drawable import Drawable
+from math import pi, cos, sin
+
+from simu.view.table_eurobot2009 import puck_attr
+
+class Bridge (Drawable):
+
+ width = 220
+ height = 100
+
+ def __init__ (self, onto, model):
+ Drawable.__init__ (self, onto)
+ self.model = model
+ self.model.register (self.update)
+ self.update ()
+
+ def draw (self):
+ self.reset ()
+ self.trans_rotate (0.95 * pi / 2)
+ self.draw_line ((-40, -80), (-40, 80), fill = '#808080')
+ self.draw_line ((40, -80), (40, 80), fill = '#808080')
+ for i in range (2):
+ puck = self.model.bridge_slot[i]
+ if puck is not None:
+ self.draw_circle ((0, 40 - 80 * i), 35,
+ **puck_attr[puck.color])
+ if self.model.bridge_door_servo_value is not None:
+ self.draw_arc ((0, -40), 37.5, start = -3 * pi / 4
+ + self.model.bridge_door_servo_value * pi, extent = pi / 2)
+ if self.model.bridge_finger_servo_value is not None:
+ self.draw_arc ((-40, -50), 50, start = pi / 2, extent = -3 * pi / 4,
+ style = 'arc', outline = '#c0c0c0')
+ a = pi / 2 - self.model.bridge_finger_servo_value * 3 * pi / 4
+ self.draw_line ((-40, -50), (-40 + 50 * cos (a), -50 + 50 * sin (a)))
+