summaryrefslogtreecommitdiff
path: root/host/simu
diff options
context:
space:
mode:
authorNicolas Schodet2012-03-30 23:09:45 +0200
committerNicolas Schodet2012-04-02 00:51:50 +0200
commitd66664cde28a490759fd6f3a11791b9f20c1933d (patch)
tree983f7d2b777a127963d0392c73c3ac7fe71d3db1 /host/simu
parentc0640f4a2598a8fcc9c90d484c0d8f0b54f03b31 (diff)
host/simu/robots/guybrush: simulate lower clamps elements taking
Diffstat (limited to 'host/simu')
-rw-r--r--host/simu/robots/guybrush/model/clamps.py73
-rw-r--r--host/simu/robots/guybrush/view/bag.py2
-rw-r--r--host/simu/robots/guybrush/view/clamps.py5
-rw-r--r--host/simu/robots/guybrush/view/robot.py18
4 files changed, 95 insertions, 3 deletions
diff --git a/host/simu/robots/guybrush/model/clamps.py b/host/simu/robots/guybrush/model/clamps.py
index b5278694..91e24f9b 100644
--- a/host/simu/robots/guybrush/model/clamps.py
+++ b/host/simu/robots/guybrush/model/clamps.py
@@ -23,6 +23,8 @@
# }}}
"""Guybrush clamps."""
from utils.observable import Observable
+from simu.utils.trans_matrix import TransMatrix
+from math import pi
class Clamps (Observable):
@@ -34,9 +36,15 @@ class Clamps (Observable):
self.lower_clamp_motor = lower_clamp_motor
self.lower_clamp_cylinders = lower_clamp_cylinders
self.lower_clamp_clamping = [ None, None ]
+ self.lower_clamp_content = [ [ ], [ ] ]
self.lower_clamp_motor.register (self.__lower_clamp_notified)
for c in self.lower_clamp_cylinders:
c.register (self.__lower_clamp_notified)
+ self.robot_position.register (self.__robot_position_notified)
+
+ def __robot_position_notified (self):
+ if self.__compute_lower_clamp ():
+ self.notify ()
def __lower_clamp_notified (self):
self.lower_clamp_rotation = self.lower_clamp_motor.angle
@@ -46,4 +54,69 @@ class Clamps (Observable):
else:
self.lower_clamp_clamping[i] = ((c.pos - c.pos_in)
/ (c.pos_out - c.pos_in))
+ self.__compute_lower_clamp ()
self.notify ()
+
+ def __compute_lower_clamp (self):
+ """Compute all operations of lower clamps."""
+ changed = False
+ if self.lower_clamp_rotation is None:
+ return False
+ # If a lower clamp is on the floor:
+ rot_mod = self.lower_clamp_rotation % (2 * pi)
+ if rot_mod < pi * 0.2 or rot_mod >= pi * 1.9:
+ floor_clamp = 0
+ elif rot_mod >= pi * 0.9 and rot_mod < pi * 1.2:
+ floor_clamp = 1
+ else:
+ floor_clamp = None
+ if floor_clamp is not None:
+ top_clamp = 1 - floor_clamp
+ # Examine floor and add found element to the clamp content.
+ for e, y in self.__get_floor_elements ():
+ self.lower_clamp_content[floor_clamp].append ((e, y))
+ e.pos = None
+ e.notify ()
+ changed = True
+ # Drop element if possible.
+ if self.lower_clamp_clamping[top_clamp] > 0.1:
+ self.__add_load ([ e
+ for e, y in self.lower_clamp_content[top_clamp]])
+ self.lower_clamp_content[top_clamp] = [ ]
+ changed = True
+ # If something is found, there can be updates.
+ if changed:
+ pass
+ return changed
+
+ def __add_load (self, elements):
+ """Add element list to load."""
+ pass
+
+ def __get_floor_elements (self):
+ """Return an elements in front of the robot, on the floor, with its y
+ coordinate in robot base."""
+ if self.robot_position.pos is None:
+ return
+ # Matrix to transform an obstacle position into robot coordinates.
+ m = self.__get_robot_matrix ()
+ # Look up elements.
+ xoffset = 117 - 6
+ xmargin = 20
+ ymargin = 171.5 - 60
+ for o in self.table.obstacles:
+ if o.level == 1 and o.value <= 1 and o.pos is not None:
+ pos = m.apply (o.pos)
+ if (pos[0] > xoffset + o.radius - xmargin
+ and pos[0] < xoffset + o.radius + xmargin
+ and pos[1] > -ymargin and pos[1] < ymargin):
+ yield (o, pos[1])
+
+ def __get_robot_matrix (self):
+ """Return robot transformation matrix."""
+ m = TransMatrix ()
+ m.rotate (-self.robot_position.angle)
+ m.translate ((-self.robot_position.pos[0],
+ -self.robot_position.pos[1]))
+ return m
+
diff --git a/host/simu/robots/guybrush/view/bag.py b/host/simu/robots/guybrush/view/bag.py
index f512886a..3d5fb621 100644
--- a/host/simu/robots/guybrush/view/bag.py
+++ b/host/simu/robots/guybrush/view/bag.py
@@ -37,7 +37,7 @@ class Bag:
'Color')
self.strat_switch = Switch (sensor_frame, model_bag.strat_switch,
'Strat')
- self.robot = Robot (table, model_bag.position)
+ self.robot = Robot (table, model_bag.position, model_bag.clamps)
self.clamps = ClampsSide (actuator_view.add_view (ClampsSide.width,
ClampsSide.height), model_bag.clamps)
self.distance_sensor = [DistanceSensorUS (self.robot, ds)
diff --git a/host/simu/robots/guybrush/view/clamps.py b/host/simu/robots/guybrush/view/clamps.py
index 11ef09b8..bdcb08a4 100644
--- a/host/simu/robots/guybrush/view/clamps.py
+++ b/host/simu/robots/guybrush/view/clamps.py
@@ -55,6 +55,11 @@ class ClampsSide (Drawable):
if c is not None:
self.draw_line ((7, -45), (7, -46 - c * 33),
(-28, -46 - c * 33))
+ if self.model.lower_clamp_content[i]:
+ e, y = self.model.lower_clamp_content[i][0]
+ self.draw_rectangle ((7, -45 - c * 15),
+ (7 - e.radius * 2, -49 - c * 15),
+ fill = GREY)
if not i:
self.trans_rotate (pi)
self.trans_pop ()
diff --git a/host/simu/robots/guybrush/view/robot.py b/host/simu/robots/guybrush/view/robot.py
index fcf375cb..79c61d15 100644
--- a/host/simu/robots/guybrush/view/robot.py
+++ b/host/simu/robots/guybrush/view/robot.py
@@ -23,18 +23,21 @@
# }}}
"""Guybrush robot view."""
import simu.inter.drawable
-from math import pi
+from math import pi, cos
+from simu.view.table_eurobot2012 import WHITE, BLACK
COLOR_ROBOT = '#000000'
COLOR_AXES = '#202040'
class Robot (simu.inter.drawable.Drawable):
- def __init__ (self, onto, position_model):
+ def __init__ (self, onto, position_model, clamps_model):
"""Construct and make connections."""
simu.inter.drawable.Drawable.__init__ (self, onto)
self.position_model = position_model
+ self.clamps_model = clamps_model
self.position_model.register (self.__position_notified)
+ self.clamps_model.register (self.update)
def __position_notified (self):
"""Called on position modifications."""
@@ -48,6 +51,17 @@ class Robot (simu.inter.drawable.Drawable):
if self.pos is not None:
self.trans_translate (self.pos)
self.trans_rotate (self.angle)
+ # Draw lower clamps content.
+ r = self.clamps_model.lower_clamp_rotation
+ if r is not None:
+ sx = cos (r)
+ for content in self.clamps_model.lower_clamp_content:
+ for e, y in content:
+ x = 117 + sx * 54
+ color = WHITE if e.value else BLACK
+ self.draw_oval ((x, y), sx * e.radius, e.radius,
+ fill = color)
+ sx = - sx
# Draw robot body.
self.draw_polygon ((150, 171.5), (-80, 171.5), (-130, 121.5),
(-130, -121.5), (-80, -171.5), (150, -171.5),