From 7c8f2999d8a61fa4f7666484a9ead9541121d4c0 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 21 Apr 2012 09:12:48 +0200 Subject: host/simu/robots/guybrush: add load/unload simulation --- digital/ai/tools/test_simu_control_guybrush.py | 4 ++++ host/simu/robots/guybrush/model/bag.py | 5 ++++- host/simu/robots/guybrush/model/clamps.py | 21 +++++++++++++++++++-- host/simu/robots/guybrush/view/clamps.py | 23 ++++++++++++++++++++++- 4 files changed, 49 insertions(+), 4 deletions(-) diff --git a/digital/ai/tools/test_simu_control_guybrush.py b/digital/ai/tools/test_simu_control_guybrush.py index b9106eab..9be6a7b0 100644 --- a/digital/ai/tools/test_simu_control_guybrush.py +++ b/digital/ai/tools/test_simu_control_guybrush.py @@ -33,6 +33,8 @@ class TestSimuControl (TestSimu): UPPER_CLAMP_IN = 1 << 3 UPPER_CLAMP_DOWN = 1 << 4 UPPER_CLAMP_UP = 1 << 5 + DOOR_OPEN = 1 << 6 + DOOR_CLOSE = 1 << 7 LOWER_CLAMP_1_OPEN = 1 << 8 LOWER_CLAMP_2_OPEN = 1 << 9 LOWER_CLAMP_ROTATION_STROKE = int (16 * 250) @@ -46,6 +48,7 @@ class TestSimuControl (TestSimu): self.robot_model = self.robots[0].model self.io.output (self.UPPER_CLAMP_UP, 'toggle') self.io.output (self.UPPER_CLAMP_IN, 'toggle') + self.io.output (self.DOOR_CLOSE, 'toggle') def create_widgets (self): TestSimu.create_widgets (self) @@ -69,6 +72,7 @@ class TestSimuControl (TestSimu): out_button ('UClamp in/out', self.UPPER_CLAMP_IN | self.UPPER_CLAMP_OUT) out_button ('UClamp open', self.UPPER_CLAMP_OPEN) + out_button ('Door open', self.DOOR_OPEN | self.DOOR_CLOSE) self.backward_var = IntVar () self.backward_button = Checkbutton (self.control_frame, text = 'Backward', variable = self.backward_var) diff --git a/host/simu/robots/guybrush/model/bag.py b/host/simu/robots/guybrush/model/bag.py index 9b05210c..a26c0f19 100644 --- a/host/simu/robots/guybrush/model/bag.py +++ b/host/simu/robots/guybrush/model/bag.py @@ -55,7 +55,10 @@ class Bag: PneumaticCylinder (output[3], output[2], scheduler, 0., 1., 1., 1., 0.), PneumaticCylinder (None, output[1], scheduler, - 0., 30., 150., 75., 30.)) + 0., 30., 150., 75., 30.), + PneumaticCylinder (output[7], output[6], scheduler, + 0., 1., 1., 1., 1., contact[7], contact[6]) + ) def distance_sensor_exclude (o): return o is self.beacon self.distance_sensor = [ diff --git a/host/simu/robots/guybrush/model/clamps.py b/host/simu/robots/guybrush/model/clamps.py index d1545c8d..5d75ecb4 100644 --- a/host/simu/robots/guybrush/model/clamps.py +++ b/host/simu/robots/guybrush/model/clamps.py @@ -24,14 +24,16 @@ """Guybrush clamps.""" from utils.observable import Observable from simu.utils.trans_matrix import TransMatrix +from simu.utils.vector import vector from math import pi +import random class Clamps (Observable): def __init__ (self, table, robot_position, lower_clamp_motor, lower_clamp_cylinders, lower_clamp_sensors, upper_clamp_up_down_cylinder, upper_clamp_in_out_cylinder, - upper_clamp_open_cylinder): + upper_clamp_open_cylinder, door_cylinder): Observable.__init__ (self) self.table = table self.robot_position = robot_position @@ -44,12 +46,15 @@ class Clamps (Observable): self.upper_clamp_in_out_cylinder = upper_clamp_in_out_cylinder self.upper_clamp_open_cylinder = upper_clamp_open_cylinder self.upper_clamp_content = [ ] + self.door_cylinder = door_cylinder + self.load = [ ] self.lower_clamp_motor.register (self.__lower_clamp_notified) for c in self.lower_clamp_cylinders: c.register (self.__lower_clamp_notified) self.upper_clamp_up_down_cylinder.register (self.__upper_clamp_notified) self.upper_clamp_in_out_cylinder.register (self.__upper_clamp_notified) self.upper_clamp_open_cylinder.register (self.__upper_clamp_notified) + self.door_cylinder.register (self.__door_notified) self.robot_position.register (self.__robot_position_notified) def __robot_position_notified (self): @@ -78,6 +83,18 @@ class Clamps (Observable): self.__compute_upper_clamp () self.notify () + def __door_notified (self): + self.door = self.door_cylinder.pos # 1. is open. + if self.door > 0.5 and self.load: + for e in self.load: + e.pos = (vector (self.robot_position.pos) + - vector.polar (self.robot_position.angle + + random.uniform (-pi/8, pi/8), + 200 + random.uniform (0, 70))) + e.notify () + self.load = [ ] + self.notify () + def __compute_lower_clamp (self): """Compute all operations of lower clamps.""" changed = False @@ -138,7 +155,7 @@ class Clamps (Observable): def __add_load (self, elements): """Add element list to load.""" - pass + self.load.extend (elements) def __get_floor_elements (self): """Return an elements in front of the robot, on the floor, with its y diff --git a/host/simu/robots/guybrush/view/clamps.py b/host/simu/robots/guybrush/view/clamps.py index 25d3f107..9ac082d7 100644 --- a/host/simu/robots/guybrush/view/clamps.py +++ b/host/simu/robots/guybrush/view/clamps.py @@ -24,7 +24,9 @@ """Guybrush clamps.""" from simu.inter.drawable import Drawable from math import pi, cos, sin, radians -from simu.view.table_eurobot2012 import YELLOW +from simu.view.table_eurobot2012 import WHITE, BLACK, YELLOW + +COIN_BLACK = BLACK GREY = '#808080' BLACK = '#000000' @@ -44,6 +46,25 @@ class ClampsSide (Drawable): # Draw base from side. self.trans_translate ((50, -150)) self.draw_line ((-93, 0), (130, 0), fill = GREY) + # Draw load. + self.trans_push () + self.trans_translate ((60, 0)) + for i, e in enumerate (self.model.load): + if e.value == 3: + self.draw_polygon ((-75, 0), (-75 + 13, 48.5), + (75 - 13, 48.5), (75, 0), fill = YELLOW, + outline = BLACK) + else: + color = WHITE if e.value else COIN_BLACK + self.draw_oval ((0, 15), 60, 15, fill = color) + self.trans_translate ((0, 15)) + self.trans_pop () + # Draw doors. + if self.model.door is not None: + a = self.model.door * 200 + b = 100 + self.model.door * 100 + self.draw_line ((130, a), (130, a + 100)) + self.draw_line ((125, b), (125, b + 100)) # Draw lower clamp. if self.model.lower_clamp_rotation is not None: self.trans_push () -- cgit v1.2.3