From 9d390fd156f3e07714d3993a73878b1d41a282d0 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Wed, 13 May 2009 18:56:01 +0200 Subject: * host/simu: - added AquaJim bridge model and view. --- host/simu/robots/aquajim/model/sorter.py | 39 ++++++++++++++++++++- host/simu/robots/aquajim/view/bag.py | 3 ++ host/simu/robots/aquajim/view/bridge.py | 59 ++++++++++++++++++++++++++++++++ 3 files changed, 100 insertions(+), 1 deletion(-) create mode 100644 host/simu/robots/aquajim/view/bridge.py (limited to 'host/simu') 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))) + -- cgit v1.2.3