summaryrefslogtreecommitdiff
path: root/host/simu/robots/apbirthday
diff options
context:
space:
mode:
Diffstat (limited to 'host/simu/robots/apbirthday')
-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
4 files changed, 123 insertions, 2 deletions
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)