summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
authorNicolas Schodet2013-04-15 22:24:42 +0200
committerNicolas Schodet2013-04-15 22:25:39 +0200
commita32cf498ca953e0ebc0558958977ecd37fc741a5 (patch)
tree66db0ff683cf19dd375e63b2923c46b995763549
parent8d625a7761959fc53b6e8d99c8c30c8456211555 (diff)
host/simu, host/simu/robots/apbirthday: add plate loading simulation
-rw-r--r--digital/ai/tools/test_simu_control_apbirthday.py6
-rw-r--r--host/simu/model/rectangular_obstacle.py11
-rw-r--r--host/simu/model/table_eurobot2013.py7
-rw-r--r--host/simu/robots/apbirthday/model/bag.py11
-rw-r--r--host/simu/robots/apbirthday/model/cannon.py92
-rw-r--r--host/simu/robots/apbirthday/view/bag.py3
-rw-r--r--host/simu/robots/apbirthday/view/robot.py19
-rw-r--r--host/simu/view/table_eurobot2013.py21
8 files changed, 145 insertions, 25 deletions
diff --git a/digital/ai/tools/test_simu_control_apbirthday.py b/digital/ai/tools/test_simu_control_apbirthday.py
index d88d5f7b..6260ffa0 100644
--- a/digital/ai/tools/test_simu_control_apbirthday.py
+++ b/digital/ai/tools/test_simu_control_apbirthday.py
@@ -37,8 +37,8 @@ class TestSimuControl (TestSimu):
self.asserv = self.robots[0].asserv
self.robot_model = self.robots[0].model
self.io.output (io_hub.apbirthday.output_mask (
- 'cake_arm_in', 'cake_push_far_in', 'cake_push_near_in'),
- 'toggle')
+ 'cake_arm_in', 'cake_push_far_in', 'cake_push_near_in',
+ 'cherry_plate_down'), 'toggle')
def create_widgets (self):
TestSimu.create_widgets (self)
@@ -59,6 +59,8 @@ class TestSimuControl (TestSimu):
out_button ('Arm in/out', 'cake_arm_in', 'cake_arm_out')
out_button ('Push far in/out', 'cake_push_far_in', 'cake_push_far_out')
out_button ('Push near in/out', 'cake_push_near_in', 'cake_push_near_out')
+ out_button ('Plate arm up/down', 'cherry_plate_up', 'cherry_plate_down')
+ out_button ('Plate clamp', 'cherry_plate_clamp')
self.backward_var = IntVar ()
self.backward_button = Checkbutton (self.control_frame,
text = 'Backward', variable = self.backward_var)
diff --git a/host/simu/model/rectangular_obstacle.py b/host/simu/model/rectangular_obstacle.py
index 71cfdc8b..8f574f01 100644
--- a/host/simu/model/rectangular_obstacle.py
+++ b/host/simu/model/rectangular_obstacle.py
@@ -61,3 +61,14 @@ class RectangularObstacle (Observable):
found = i
return found
+ def inside (self, a):
+ """If A is inside obstacle, return True."""
+ # Map point in obstacle coordinates.
+ u = vector.polar (self.angle, 1)
+ o = vector (self.pos)
+ a = vector (a)
+ oa = a - o
+ x = oa * u / (.5 * self.dim[0])
+ y = oa * u.normal () / (.5 * self.dim[1])
+ return x > -1 and x < 1 and y > -1 and y < 1
+
diff --git a/host/simu/model/table_eurobot2013.py b/host/simu/model/table_eurobot2013.py
index 8e5e83fc..d73b0564 100644
--- a/host/simu/model/table_eurobot2013.py
+++ b/host/simu/model/table_eurobot2013.py
@@ -76,11 +76,11 @@ class Table (simu.model.table.Table):
add_glass ((1200, 1050))
# Cherries.
self.plates = [ ]
- self.cherries = [ ]
def add_plate (pos, color):
plate = RectangularObstacle ((170, 170), 0)
plate.pos = pos
plate.angle = 0
+ plate.cherries = [ ]
self.plates.append (plate)
cpos = ((-42, -42), (-42, 0), (-42, +42), (0, -21), (0, 21),
(42, -42), (42, 0), (42, +42))
@@ -88,9 +88,9 @@ class Table (simu.model.table.Table):
random.shuffle (ccol)
for p, c in zip (cpos, ccol):
cherry = RoundObstacle (20, 0)
- cherry.pos = (pos[0] + p[0], pos[1] + p[1])
+ cherry.pos = p
cherry.color = c
- self.cherries.append (cherry)
+ plate.cherries.append (cherry)
for py in (250, 600, 1000, 1400, 1750):
add_plate ((200, py), False)
add_plate ((3000 - 200, py), True)
@@ -114,6 +114,5 @@ class Table (simu.model.table.Table):
self.obstacles += self.candles
self.obstacles += self.glasses
self.obstacles += self.plates
- self.obstacles += self.cherries
self.obstacles += self.gifts
diff --git a/host/simu/robots/apbirthday/model/bag.py b/host/simu/robots/apbirthday/model/bag.py
index 28295e63..2043270b 100644
--- a/host/simu/robots/apbirthday/model/bag.py
+++ b/host/simu/robots/apbirthday/model/bag.py
@@ -29,6 +29,7 @@ from simu.model.distance_sensor_trig import DistanceSensorTrig
from simu.model.distance_sensor_sensopart import DistanceSensorSensopart
from simu.model.pneumatic_cylinder import PneumaticCylinder
from simu.robots.apbirthday.model.cake_arm import CakeArm
+from simu.robots.apbirthday.model.cannon import Cannon
from math import pi
import random
@@ -72,5 +73,15 @@ class Bag:
link_bag.cake_push_near_out,
scheduler, 0., 1., 10., 10., 0.),
link_bag.cake_arm_out_contact, link_bag.cake_arm_in_contact)
+ self.cannon = Cannon (table, self.position,
+ PneumaticCylinder (
+ link_bag.cherry_plate_up,
+ link_bag.cherry_plate_down,
+ scheduler, 0., 1., 2., 2., 1.),
+ PneumaticCylinder (None,
+ link_bag.cherry_plate_clamp,
+ scheduler, 0., 1., 10., 10., 0.),
+ (Switch (link_bag.cherry_plate_left_contact),
+ Switch (link_bag.cherry_plate_right_contact)))
self.pos_report = link_bag.io_hub.pos_report
diff --git a/host/simu/robots/apbirthday/model/cannon.py b/host/simu/robots/apbirthday/model/cannon.py
new file mode 100644
index 00000000..74810fd6
--- /dev/null
+++ b/host/simu/robots/apbirthday/model/cannon.py
@@ -0,0 +1,92 @@
+# simu - Robot simulation. {{{
+#
+# Copyright (C) 2013 Nicolas Schodet
+#
+# APBTeam:
+# Web: http://apbteam.org/
+# Email: team AT apbteam DOT org
+#
+# This program is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+#
+# }}}
+"""APBirthday cherry cannon."""
+from utils.observable import Observable
+from simu.utils.trans_matrix import TransMatrix
+from simu.utils.vector import vector
+
+class Cannon (Observable):
+
+ def __init__ (self, table, robot_position,
+ arm_cyl, clamp_cyl, contacts):
+ Observable.__init__ (self)
+ self.table = table
+ self.robot_position = robot_position
+ self.arm_cyl = arm_cyl
+ self.clamp_cyl = clamp_cyl
+ self.contacts = contacts
+ self.plate = None
+ self.cherries = [ ]
+ self.robot_position.register (self.__robot_position_notified)
+ self.arm_cyl.register (self.__arm_notified)
+ self.clamp_cyl.register (self.__arm_notified)
+
+ def __robot_position_notified (self):
+ if self.robot_position.pos is None:
+ return
+ m = TransMatrix ()
+ m.translate (self.robot_position.pos)
+ m.rotate (self.robot_position.angle)
+ x = -108
+ y = (50, -50)
+ for i, c in enumerate (self.contacts):
+ s = True
+ sensor_pos = m.apply ((x, y[i]))
+ for o in self.table.obstacles:
+ if (o.pos is not None and o.pos[1] > 0
+ and hasattr (o, 'inside') and o.inside (sensor_pos)):
+ s = False
+ break
+ if s != self.contacts[i].state:
+ self.contacts[i].state = s
+ self.contacts[i].notify ()
+
+ def __arm_notified (self):
+ if self.robot_position.pos is None:
+ return
+ if (self.plate is None and self.arm_cyl.pos > 0.9
+ and self.clamp_cyl.pos > 0.9):
+ # Pick plate.
+ self.plate = self.table.nearest (self.__plate_drop_point (),
+ level = 0, max = 50)
+ if self.plate is not None:
+ self.plate.pos = None
+ self.plate.notify ()
+ elif self.plate is not None and self.clamp_cyl.pos < 0.9:
+ # Drop plate.
+ self.plate.pos = self.__plate_drop_point ()
+ self.plate.angle = self.robot_position.angle
+ self.plate.notify ()
+ self.plate = None
+ elif (self.plate is not None and self.plate.cherries and
+ self.arm_cyl.pos < .1):
+ # Load cherries.
+ self.cherries = self.plate.cherries
+ self.plate.cherries = [ ]
+ self.notify ()
+
+ def __plate_drop_point (self):
+ return (vector (self.robot_position.pos)
+ - vector.polar (self.robot_position.angle, 108 + 85))
+
diff --git a/host/simu/robots/apbirthday/view/bag.py b/host/simu/robots/apbirthday/view/bag.py
index d5c1744d..05dbfc70 100644
--- a/host/simu/robots/apbirthday/view/bag.py
+++ b/host/simu/robots/apbirthday/view/bag.py
@@ -38,7 +38,8 @@ class Bag:
'Strat')
self.robot_nb_switch = Switch (sensor_frame,
model_bag.robot_nb_switch, 'Nb robots')
- self.robot = Robot (table, model_bag.position, model_bag.cake_arm)
+ self.robot = Robot (table, model_bag.position, model_bag.cake_arm,
+ model_bag.cannon)
self.distance_sensor = [DistanceSensorUS (self.robot, ds)
for ds in model_bag.distance_sensor]
self.cake_front = DistanceSensor (self.robot, model_bag.cake_front)
diff --git a/host/simu/robots/apbirthday/view/robot.py b/host/simu/robots/apbirthday/view/robot.py
index cb489fb0..e4153232 100644
--- a/host/simu/robots/apbirthday/view/robot.py
+++ b/host/simu/robots/apbirthday/view/robot.py
@@ -23,19 +23,22 @@
# }}}
"""APBirthday robot view."""
import simu.inter.drawable
+from simu.view.table_eurobot2013 import PINK, colors
COLOR_ROBOT = '#000000'
COLOR_AXES = '#202040'
class Robot (simu.inter.drawable.Drawable):
- def __init__ (self, onto, position_model, cake_arm_model):
+ def __init__ (self, onto, position_model, cake_arm_model, cannon_model):
"""Construct and make connections."""
simu.inter.drawable.Drawable.__init__ (self, onto)
self.position_model = position_model
self.position_model.register (self.__position_notified)
self.cake_arm_model = cake_arm_model
self.cake_arm_model.register (self.update)
+ self.cannon_model = cannon_model
+ self.cannon_model.register (self.update)
def __position_notified (self):
"""Called on position modifications."""
@@ -49,6 +52,20 @@ class Robot (simu.inter.drawable.Drawable):
if self.pos is not None:
self.trans_translate (self.pos)
self.trans_rotate (self.angle)
+ # Draw plate.
+ plate = self.cannon_model.plate
+ f = self.cannon_model.arm_cyl.pos
+ if plate is not None:
+ self.draw_rectangle ((-108 - f * 170, 85), (-108, -85),
+ fill = PINK)
+ self.draw_rectangle ((-108 - f * 148, 85 - 22),
+ (-108 - f * 22, -85 + 22), fill = PINK)
+ for c in plate.cherries:
+ if c.pos:
+ self.draw_circle ((-108 - f * (c.pos[0] + 85),
+ c.pos[1]), c.radius, fill = colors[c.color])
+ self.draw_rectangle ((-108 - f * 170, 85),
+ (-108 - f * 170 - (1 - f) * 22, -85), fill = PINK)
# Draw robot body.
self.draw_polygon ((102, 140), (102, -140), (-108, -140),
(-108, 70), (-58, 140), fill = COLOR_ROBOT)
diff --git a/host/simu/view/table_eurobot2013.py b/host/simu/view/table_eurobot2013.py
index 889fe369..6acd847d 100644
--- a/host/simu/view/table_eurobot2013.py
+++ b/host/simu/view/table_eurobot2013.py
@@ -79,26 +79,15 @@ class Plate (Drawable):
self.reset ()
if self.model.pos:
self.trans_translate (self.model.pos)
+ self.trans_rotate (self.model.angle)
w, h = self.model.dim
w, h = w/2, h/2
self.draw_rectangle ((-w, -h), (w, h), fill = PINK)
self.draw_rectangle ((-w + 22, -h + 22), (w - 22, h - 22),
fill = PINK)
- Drawable.draw (self)
-
-class Cherry (Drawable):
-
- def __init__ (self, onto, model):
- Drawable.__init__ (self, onto)
- self.model = model
- self.model.register (self.update)
-
- def draw (self):
- self.reset ()
- model = self.model
- if model.pos:
- self.draw_circle (model.pos, model.radius,
- fill = colors[model.color])
+ for c in self.model.cherries:
+ if c.pos:
+ self.draw_circle (c.pos, c.radius, fill = colors[c.color])
Drawable.draw (self)
class Gift (Drawable):
@@ -133,8 +122,6 @@ class Table (Drawable):
Glass (self, e)
for e in self.model.plates:
Plate (self, e)
- for e in self.model.cherries:
- Cherry (self, e)
for e in self.model.gifts:
Gift (self, e)