From 7bee1130c8d2eb1fc122bbf7867d15e413292ec2 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 8 May 2012 01:11:29 +0200 Subject: digital/io-hub/src/guybrush, host/simu/robots/guybrush: add lower clamp zero --- host/simu/robots/guybrush/model/bag.py | 2 +- host/simu/robots/guybrush/model/clamps.py | 9 ++++++++- 2 files changed, 9 insertions(+), 2 deletions(-) (limited to 'host/simu/robots/guybrush/model') diff --git a/host/simu/robots/guybrush/model/bag.py b/host/simu/robots/guybrush/model/bag.py index a6302f44..ec83cb29 100644 --- a/host/simu/robots/guybrush/model/bag.py +++ b/host/simu/robots/guybrush/model/bag.py @@ -59,7 +59,7 @@ class Bag: None, scheduler, 0., 30., 150., 75., 30.), PneumaticCylinder (output[self.OUTPUT_LOWER_CLAMP_2_CLOSE], None, scheduler, 0., 30., 150., 75., 30.)), - contact[0:4], + contact[0:4], contact[10], PneumaticCylinder (output[self.OUTPUT_UPPER_CLAMP_DOWN], output[self.OUTPUT_UPPER_CLAMP_UP], scheduler, 0., 1., 1., 1., 1., contact[4], contact[5]), diff --git a/host/simu/robots/guybrush/model/clamps.py b/host/simu/robots/guybrush/model/clamps.py index d723847a..22444f30 100644 --- a/host/simu/robots/guybrush/model/clamps.py +++ b/host/simu/robots/guybrush/model/clamps.py @@ -31,7 +31,7 @@ import random class Clamps (Observable): def __init__ (self, table, robot_position, lower_clamp_motor, - lower_clamp_cylinders, lower_clamp_sensors, + lower_clamp_cylinders, lower_clamp_sensors, lower_clamp_zero, upper_clamp_up_down_cylinder, upper_clamp_in_out_cylinder, upper_clamp_open_cylinder, door_cylinder): Observable.__init__ (self) @@ -40,6 +40,7 @@ class Clamps (Observable): self.lower_clamp_motor = lower_clamp_motor self.lower_clamp_cylinders = lower_clamp_cylinders self.lower_clamp_sensors = lower_clamp_sensors + self.lower_clamp_zero = lower_clamp_zero self.lower_clamp_clamping = [ None, None ] self.lower_clamp_content = [ [ ], [ ] ] self.upper_clamp_up_down_cylinder = upper_clamp_up_down_cylinder @@ -132,6 +133,12 @@ class Clamps (Observable): for s in self.lower_clamp_sensors: s.state = new_state s.notify () + old_state = self.lower_clamp_zero.state + new_state = (rot_mod >= pi * 1.45 and rot_mod < pi * 1.55 + and self.lower_clamp_clamping[0]) + if new_state != old_state: + self.lower_clamp_zero.state = new_state + self.lower_clamp_zero.notify () return changed def __compute_upper_clamp (self): -- cgit v1.2.3