summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--digital/ai/tools/test_simu_control_guybrush.py28
-rw-r--r--host/simu/robots/guybrush/model/bag.py17
-rw-r--r--host/simu/robots/guybrush/model/clamps.py67
-rw-r--r--host/simu/robots/guybrush/view/clamps.py33
-rw-r--r--host/simu/robots/guybrush/view/robot.py43
5 files changed, 171 insertions, 17 deletions
diff --git a/digital/ai/tools/test_simu_control_guybrush.py b/digital/ai/tools/test_simu_control_guybrush.py
index 6d742e90..b9106eab 100644
--- a/digital/ai/tools/test_simu_control_guybrush.py
+++ b/digital/ai/tools/test_simu_control_guybrush.py
@@ -28,6 +28,11 @@ import math
class TestSimuControl (TestSimu):
"""Interface with extra control."""
+ UPPER_CLAMP_OPEN = 1 << 1
+ UPPER_CLAMP_OUT = 1 << 2
+ UPPER_CLAMP_IN = 1 << 3
+ UPPER_CLAMP_DOWN = 1 << 4
+ UPPER_CLAMP_UP = 1 << 5
LOWER_CLAMP_1_OPEN = 1 << 8
LOWER_CLAMP_2_OPEN = 1 << 9
LOWER_CLAMP_ROTATION_STROKE = int (16 * 250)
@@ -39,24 +44,31 @@ class TestSimuControl (TestSimu):
self.asserv = self.robots[0].asserv
self.mimot = self.robots[0].mimot
self.robot_model = self.robots[0].model
+ self.io.output (self.UPPER_CLAMP_UP, 'toggle')
+ self.io.output (self.UPPER_CLAMP_IN, 'toggle')
def create_widgets (self):
TestSimu.create_widgets (self)
self.control_frame = Frame (self)
self.control_frame.pack (side = 'left', before = self.table_view,
fill = 'y')
- self.lower_clamp_1_open_button = Button (self.control_frame,
- text = 'LClamp 1 open', padx = 0, pady = 0,
- command = self.lower_clamp_1_open_command)
- self.lower_clamp_1_open_button.pack ()
- self.lower_clamp_2_open_button = Button (self.control_frame,
- text = 'LClamp 2 open', padx = 0, pady = 0,
- command = self.lower_clamp_2_open_command)
- self.lower_clamp_2_open_button.pack ()
+ def out_button (name, toggle):
+ def command ():
+ self.io.output (toggle, 'toggle')
+ button = Button (self.control_frame, text = name,
+ padx = 0, pady = 0, command = command)
+ button.pack ()
+ out_button ('LClamp 1 open', self.LOWER_CLAMP_1_OPEN)
+ out_button ('LClamp 2 open', self.LOWER_CLAMP_2_OPEN)
self.lower_clamp_rotate_button = Button (self.control_frame,
text = 'LClamp rotate', padx = 0, pady = 0,
command = self.lower_clamp_rotate_command)
self.lower_clamp_rotate_button.pack ()
+ out_button ('UClamp up/down',
+ self.UPPER_CLAMP_UP | self.UPPER_CLAMP_DOWN)
+ out_button ('UClamp in/out',
+ self.UPPER_CLAMP_IN | self.UPPER_CLAMP_OUT)
+ out_button ('UClamp open', self.UPPER_CLAMP_OPEN)
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 3b6cc857..8e7250b2 100644
--- a/host/simu/robots/guybrush/model/bag.py
+++ b/host/simu/robots/guybrush/model/bag.py
@@ -39,12 +39,19 @@ class Bag:
self.jack = Switch (link_bag.io_hub.contact[1], invert = True)
self.strat_switch = Switch (link_bag.io_hub.contact[2], invert = True)
self.position = Position (link_bag.asserv.position)
+ output = link_bag.io_hub.output
self.clamps = Clamps (table, self.position, link_bag.mimot.aux[0],
- (PneumaticCylinder (None, link_bag.io_hub.output[8],
- scheduler, 0., 30., 150., 75., 30.),
- PneumaticCylinder (None, link_bag.io_hub.output[9],
- scheduler, 0., 30., 150., 75., 30.)),
- [ Switch (c) for c in link_bag.io_hub.contact[3:3+4] ])
+ (PneumaticCylinder (None, output[8], scheduler,
+ 0., 30., 150., 75., 30.),
+ PneumaticCylinder (None, output[9], scheduler,
+ 0., 30., 150., 75., 30.)),
+ [ Switch (c) for c in link_bag.io_hub.contact[3:3+4] ],
+ PneumaticCylinder (output[4], output[5], scheduler,
+ 0., 1., 1., 1., 1.),
+ PneumaticCylinder (output[3], output[2], scheduler,
+ 0., 1., 1., 1., 0.),
+ PneumaticCylinder (None, output[1], scheduler,
+ 0., 30., 150., 75., 30.))
self.distance_sensor = [
DistanceSensorSensopart (link_bag.io_hub.adc[0], scheduler, table,
(20, 20), pi * 10 / 180, (self.position, ), 2),
diff --git a/host/simu/robots/guybrush/model/clamps.py b/host/simu/robots/guybrush/model/clamps.py
index 1cfc30f1..d1545c8d 100644
--- a/host/simu/robots/guybrush/model/clamps.py
+++ b/host/simu/robots/guybrush/model/clamps.py
@@ -29,7 +29,9 @@ from math import pi
class Clamps (Observable):
def __init__ (self, table, robot_position, lower_clamp_motor,
- lower_clamp_cylinders, lower_clamp_sensors):
+ lower_clamp_cylinders, lower_clamp_sensors,
+ upper_clamp_up_down_cylinder, upper_clamp_in_out_cylinder,
+ upper_clamp_open_cylinder):
Observable.__init__ (self)
self.table = table
self.robot_position = robot_position
@@ -38,9 +40,16 @@ class Clamps (Observable):
self.lower_clamp_sensors = lower_clamp_sensors
self.lower_clamp_clamping = [ None, None ]
self.lower_clamp_content = [ [ ], [ ] ]
+ self.upper_clamp_up_down_cylinder = upper_clamp_up_down_cylinder
+ 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.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.robot_position.register (self.__robot_position_notified)
def __robot_position_notified (self):
@@ -58,6 +67,17 @@ class Clamps (Observable):
self.__compute_lower_clamp ()
self.notify ()
+ def __upper_clamp_notified (self):
+ self.upper_clamp_up_down = 1 - self.upper_clamp_up_down_cylinder.pos # 1. is down.
+ self.upper_clamp_in_out = self.upper_clamp_in_out_cylinder.pos # 1. is out.
+ c = self.upper_clamp_open_cylinder
+ if c.pos is None:
+ self.upper_clamp_clamping = None
+ else:
+ self.upper_clamp_clamping = c.pos / c.pos_out
+ self.__compute_upper_clamp ()
+ self.notify ()
+
def __compute_lower_clamp (self):
"""Compute all operations of lower clamps."""
changed = False
@@ -97,6 +117,25 @@ class Clamps (Observable):
s.notify ()
return changed
+ def __compute_upper_clamp (self):
+ """Compute all operations of upper clamps."""
+ if (self.upper_clamp_up_down is None
+ or self.upper_clamp_in_out is None
+ or self.upper_clamp_clamping is None):
+ return
+ if (self.upper_clamp_clamping < 0.1
+ and self.upper_clamp_up_down > 0.9):
+ # Examine elements in front of the robot.
+ for e, y in self.__get_upper_elements ():
+ self.upper_clamp_content.append ((e, y))
+ e.pos = None
+ e.notify ()
+ if (self.upper_clamp_clamping > 0.9
+ and self.upper_clamp_up_down < 0.1):
+ # Drop elements.
+ self.__add_load ([ e for e, y in self.upper_clamp_content])
+ self.upper_clamp_content = [ ]
+
def __add_load (self, elements):
"""Add element list to load."""
pass
@@ -120,6 +159,32 @@ class Clamps (Observable):
and pos[1] > -ymargin and pos[1] < ymargin):
yield (o, pos[1])
+ def __get_upper_elements (self):
+ """Return elements in front of the robot in the upper clamps, with its
+ y coordinate in the 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 = 150
+ xmargin = 20
+ ymargin2 = 20
+ ymargin3 = 171.5
+ for o in self.table.obstacles:
+ if o.level == 2 and o.pos is not None:
+ pos = m.apply (o.pos)
+ if (pos[0] > xoffset + 35 - xmargin
+ and pos[0] < xoffset + 35 + xmargin
+ and pos[1] > -ymargin2 and pos[1] < ymargin2):
+ yield (o, 0)
+ if o.level == 3 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] > -ymargin3 and pos[1] < ymargin3):
+ yield (o, pos[1])
+
def __get_robot_matrix (self):
"""Return robot transformation matrix."""
m = TransMatrix ()
diff --git a/host/simu/robots/guybrush/view/clamps.py b/host/simu/robots/guybrush/view/clamps.py
index bdcb08a4..25d3f107 100644
--- a/host/simu/robots/guybrush/view/clamps.py
+++ b/host/simu/robots/guybrush/view/clamps.py
@@ -23,7 +23,8 @@
# }}}
"""Guybrush clamps."""
from simu.inter.drawable import Drawable
-from math import pi
+from math import pi, cos, sin, radians
+from simu.view.table_eurobot2012 import YELLOW
GREY = '#808080'
BLACK = '#000000'
@@ -63,3 +64,33 @@ class ClampsSide (Drawable):
if not i:
self.trans_rotate (pi)
self.trans_pop ()
+ # Draw upper clamp.
+ ud = self.model.upper_clamp_up_down
+ c = self.model.upper_clamp_clamping
+ io = self.model.upper_clamp_in_out
+ if (ud is not None and c is not None and io is not None):
+ self.trans_push ()
+ # Approximation: map cylinder extension to angle.
+ a = radians (ud * 75 + 87)
+ x = 40 + 188 * cos (a)
+ y = 75 + 188 * sin (a)
+ self.trans_translate ((x, y))
+ self.trans_rotate ((1 - ud) * 0.5 * pi)
+ self.draw_line ((0, 56), (0, -25), fill = GREY)
+ # Draw level 2 clamp.
+ for e, y, in self.model.upper_clamp_content:
+ if e.level == 2:
+ self.draw_polygon ((-15, -15), (-15 - 70, -15),
+ (-15 - 13 - 44, -15 + 48.5),
+ (-15 - 13, -15 + 48.8), fill = YELLOW,
+ outline = BLACK)
+ break
+ self.draw_line ((-15, -10 + c * 5), (-15 - 70, -10 + c * 5))
+ # Draw level 3 clamp.
+ for e, y in self.model.upper_clamp_content:
+ if e.level == 3:
+ self.draw_rectangle ((-12 * io - e.radius * 2, 54),
+ (-12 * io, 50), fill = GREY)
+ self.draw_line ((-12 * io - 35, 55), (-12 * io, 55),
+ (-12 * io, 55 - c * 33), (-12 * io - 35, 55 - c * 33))
+ self.trans_pop ()
diff --git a/host/simu/robots/guybrush/view/robot.py b/host/simu/robots/guybrush/view/robot.py
index 79c61d15..a7377a66 100644
--- a/host/simu/robots/guybrush/view/robot.py
+++ b/host/simu/robots/guybrush/view/robot.py
@@ -23,12 +23,14 @@
# }}}
"""Guybrush robot view."""
import simu.inter.drawable
-from math import pi, cos
-from simu.view.table_eurobot2012 import WHITE, BLACK
+from math import pi, cos, sin, radians
+from simu.view.table_eurobot2012 import WHITE, BLACK, YELLOW
COLOR_ROBOT = '#000000'
COLOR_AXES = '#202040'
+GREY = '#808080'
+
class Robot (simu.inter.drawable.Drawable):
def __init__ (self, onto, position_model, clamps_model):
@@ -62,6 +64,43 @@ class Robot (simu.inter.drawable.Drawable):
self.draw_oval ((x, y), sx * e.radius, e.radius,
fill = color)
sx = - sx
+ # Draw upper clamps.
+ ud = self.clamps_model.upper_clamp_up_down
+ c = self.clamps_model.upper_clamp_clamping
+ io = self.clamps_model.upper_clamp_in_out
+ if (ud is not None and c is not None and io is not None
+ and ud > 0.1):
+ self.trans_push ()
+ # Approximation: map cylinder extension to angle.
+ cx = -cos (radians (ud * 75 + 87))
+ x = -40 + 188 * cx
+ self.trans_translate ((x, 0))
+ # Draw level 2 clamp.
+ for e, y, in self.clamps_model.upper_clamp_content:
+ if e.level == 2:
+ self.draw_rectangle ((15 * cx, 75), (85 * cx, -75),
+ fill = YELLOW)
+ self.draw_rectangle ((28 * cx, 62), (72 * cx, -62),
+ fill = YELLOW)
+ break
+ self.draw_line ((15 * cx, 80 - c * 10), (85 * cx, 80 - c * 10))
+ self.draw_line ((15 * cx, -80 + c * 10), (85 * cx, -80 + c * 10))
+ # Draw level 3 clamp.
+ for side in (1, -1):
+ a = side * -0.61 * (1 - io)
+ rx, ry = cos (a), sin (a)
+ for e, y in self.clamps_model.upper_clamp_content:
+ if e.level == 3 and y * side > 0:
+ color = WHITE if e.value else BLACK
+ y = 80 - y * side
+ self.draw_oval ((rx * 72 + ry * y, ry * 72 + rx * y + side * 80),
+ e.radius * cx, e.radius, fill = color)
+ self.trans_push ()
+ self.trans_translate ((0, side * 80))
+ self.trans_rotate (a)
+ self.draw_rectangle ((12, -4 * side), (12 + 35, 43 * side), fill = GREY)
+ self.trans_pop ()
+ self.trans_pop ()
# Draw robot body.
self.draw_polygon ((150, 171.5), (-80, 171.5), (-130, 121.5),
(-130, -121.5), (-80, -171.5), (150, -171.5),