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 +++++++++++++++++++++++++++++++- 1 file changed, 38 insertions(+), 1 deletion(-) (limited to 'host/simu/robots/aquajim/model') 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 () + -- cgit v1.2.3