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 --- host/simu/robots/guybrush/model/bag.py | 5 ++++- host/simu/robots/guybrush/model/clamps.py | 21 +++++++++++++++++++-- host/simu/robots/guybrush/view/clamps.py | 23 ++++++++++++++++++++++- 3 files changed, 45 insertions(+), 4 deletions(-) (limited to 'host') 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