From 88f0b43e4182c156b5fbcd6f1d418e8e5b459493 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 7 Apr 2013 15:55:51 +0200 Subject: host/simu/robots/apbirthday/model: add arm contacts simulation --- host/simu/robots/apbirthday/model/bag.py | 3 ++- host/simu/robots/apbirthday/model/cake_arm.py | 20 ++++++++++++++++++-- 2 files changed, 20 insertions(+), 3 deletions(-) (limited to 'host/simu/robots/apbirthday/model') diff --git a/host/simu/robots/apbirthday/model/bag.py b/host/simu/robots/apbirthday/model/bag.py index c8d28303..ce2c8667 100644 --- a/host/simu/robots/apbirthday/model/bag.py +++ b/host/simu/robots/apbirthday/model/bag.py @@ -63,6 +63,7 @@ class Bag: PneumaticCylinder ( link_bag.cake_push_near_in, link_bag.cake_push_near_out, - scheduler, 0., 1., 10., 10., 0.)) + scheduler, 0., 1., 10., 10., 0.), + link_bag.cake_arm_out_contact, link_bag.cake_arm_in_contact) self.pos_report = link_bag.io_hub.pos_report diff --git a/host/simu/robots/apbirthday/model/cake_arm.py b/host/simu/robots/apbirthday/model/cake_arm.py index 0d91b7af..8a8d1629 100644 --- a/host/simu/robots/apbirthday/model/cake_arm.py +++ b/host/simu/robots/apbirthday/model/cake_arm.py @@ -32,20 +32,36 @@ class CakeArm (Observable): near_x = 2 near_y = 247 - def __init__ (self, table, robot_position, arm_cyl, far_cyl, near_cyl): + def __init__ (self, table, robot_position, arm_cyl, far_cyl, near_cyl, + arm_out_contact, arm_in_contact): Observable.__init__ (self) self.table = table self.robot_position = robot_position self.arm_cyl = arm_cyl self.far_cyl = far_cyl self.near_cyl = near_cyl + self.arm_out_contact = arm_out_contact + self.arm_in_contact = arm_in_contact self.far_pushed = False self.near_pushed = False - self.arm_cyl.register (self.notify) + self.arm_cyl.register (self.__arm_notified) self.far_cyl.register (self.__push_notified) self.near_cyl.register (self.__push_notified) self.robot_position.register (self.__robot_position_notified) + def __arm_notified (self): + if self.arm_cyl.pos > .9: + contacts = (False, True) + elif self.arm_cyl.pos < .1: + contacts = (True, False) + else: + contacts = (True, True) + for contact, state in ((self.arm_out_contact, contacts[0]), + (self.arm_in_contact, contacts[1])): + if contact.state != state: + contact.state = state + contact.notify () + def __push_notified (self): if self.arm_cyl.pos > .9: if not self.far_pushed and self.far_cyl.pos > .5: -- cgit v1.2.3