summaryrefslogtreecommitdiffhomepage
path: root/host/simu
diff options
context:
space:
mode:
authorNicolas Schodet2012-04-21 09:12:48 +0200
committerNicolas Schodet2012-04-21 09:12:48 +0200
commit7c8f2999d8a61fa4f7666484a9ead9541121d4c0 (patch)
tree1d3784050ca2f9e90b0c1dd0237bda789f5b927e /host/simu
parent7399342e11e0f025a1904ae5cbf5946100231472 (diff)
host/simu/robots/guybrush: add load/unload simulation
Diffstat (limited to 'host/simu')
-rw-r--r--host/simu/robots/guybrush/model/bag.py5
-rw-r--r--host/simu/robots/guybrush/model/clamps.py21
-rw-r--r--host/simu/robots/guybrush/view/clamps.py23
3 files changed, 45 insertions, 4 deletions
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 ()