From 86644789c372f141cfe5fa91f7c12a947de7b822 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 17 May 2009 22:20:46 +0200 Subject: * digital/io/tools, host/simu: - added contact simulation. --- digital/io/tools/io/mex.py | 2 +- host/simu/robots/aquajim/model/bag.py | 7 +++-- host/simu/robots/aquajim/model/sorter.py | 47 ++++++++++++++++++++++++-------- 3 files changed, 41 insertions(+), 15 deletions(-) diff --git a/digital/io/tools/io/mex.py b/digital/io/tools/io/mex.py index d3c814ef..8f951f91 100644 --- a/digital/io/tools/io/mex.py +++ b/digital/io/tools/io/mex.py @@ -42,7 +42,7 @@ ADC_NB = 6 PWM_NB = 1 PWM_VALUE_MAX = 1024 -CONTACT_NB = 2 +CONTACT_NB = 3 CONTACT_INIT = 0x3f class Mex: diff --git a/host/simu/robots/aquajim/model/bag.py b/host/simu/robots/aquajim/model/bag.py index 929d240a..ba854af5 100644 --- a/host/simu/robots/aquajim/model/bag.py +++ b/host/simu/robots/aquajim/model/bag.py @@ -39,8 +39,11 @@ class Bag: self.elevator_door = MotorBasic (link_bag.io.pwm[0], scheduler, 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.position, )) + link_bag.io.contact[2], + link_bag.io.servo[0:2], link_bag.io.contact[0], + link_bag.asserv.aux[1], self.elevator_door, + link_bag.io.contact[1], + (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 d5452aa5..2c54f141 100644 --- a/host/simu/robots/aquajim/model/sorter.py +++ b/host/simu/robots/aquajim/model/sorter.py @@ -28,8 +28,11 @@ from math import pi class Sorter (Observable): - def __init__ (self, table, arm_motor_link, elevator_motor_link, - servo_links, elevator_door_model, into = None): + def __init__ (self, table, + arm_motor_link, cylinder_puck_contact, + servo_links, bridge_puck_contact, + elevator_motor_link, elevator_door_model, elevator_door_contact, + into = None): Observable.__init__ (self) self.table = table self.into = into or () @@ -39,15 +42,20 @@ class Sorter (Observable): self.arm_motor_link = arm_motor_link self.arm_motor_link.register (self.__arm_motor_notified) self.__arm_motor_notified () - self.elevator_motor_link = elevator_motor_link - self.elevator_motor_link.register (self.__elevator_motor_notified) - self.__elevator_motor_notified () + self.cylinder_puck_contact = cylinder_puck_contact self.servo_links = servo_links + self.bridge_puck_contact = bridge_puck_contact 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_motor_link = elevator_motor_link + self.elevator_motor_link.register (self.__elevator_motor_notified) + self.__elevator_motor_notified () self.elevator_door = elevator_door_model + self.elevator_door_contact = elevator_door_contact + self.elevator_door.register (self.__elevator_door_notified) + self.__elevator_door_notified () def __transform (self, pos): m = TransMatrix () @@ -66,6 +74,9 @@ class Sorter (Observable): entry_pos = self.__transform ((150 - 40, 0)) if entry_pos is not None: front_puck = self.table.nearest (entry_pos, level = 1, max = 35) + # Update puck cylinder contact. + self.cylinder_puck_contact.state = front_puck is None + self.cylinder_puck_contact.notify () # For each arm. for i in range (3): a = (self.arm_angle + i * 2 * pi / 3) % (2 * pi) @@ -84,13 +95,6 @@ class Sorter (Observable): self.arm_slot[i] = None self.notify () - def __elevator_motor_notified (self): - if self.elevator_motor_link.angle is None: - self.elevator_height = None - else: - 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: @@ -100,6 +104,7 @@ class Sorter (Observable): # Pass the door. self.bridge_slot[1] = self.bridge_slot[0] self.bridge_slot[0] = None + self.__bridge_puck_update () self.notify () def __bridge_finger_servo_notified (self): @@ -112,5 +117,23 @@ class Sorter (Observable): # Drop until elevator is fully implemented. self.lost.append (self.bridge_slot[1]) self.bridge_slot[1] = None + self.__bridge_puck_update () self.notify () + def __bridge_puck_update (self): + # Update bridge puck contact. + self.bridge_puck_contact.state = self.bridge_slot[1] is None + self.bridge_puck_contact.notify () + + def __elevator_motor_notified (self): + if self.elevator_motor_link.angle is None: + self.elevator_height = None + else: + self.elevator_height = 150 - self.elevator_motor_link.angle * 5.5 + self.notify () + + def __elevator_door_notified (self): + self.elevator_door_contact.state = (self.elevator_door.angle + != self.elevator_door.max_stop) + self.elevator_door_contact.notify () + -- cgit v1.2.3