summaryrefslogtreecommitdiff
path: root/host/simu/robots/apbirthday/model/cannon.py
diff options
context:
space:
mode:
authorNicolas Schodet2013-04-15 22:24:42 +0200
committerNicolas Schodet2013-04-15 22:25:39 +0200
commita32cf498ca953e0ebc0558958977ecd37fc741a5 (patch)
tree66db0ff683cf19dd375e63b2923c46b995763549 /host/simu/robots/apbirthday/model/cannon.py
parent8d625a7761959fc53b6e8d99c8c30c8456211555 (diff)
host/simu, host/simu/robots/apbirthday: add plate loading simulation
Diffstat (limited to 'host/simu/robots/apbirthday/model/cannon.py')
-rw-r--r--host/simu/robots/apbirthday/model/cannon.py92
1 files changed, 92 insertions, 0 deletions
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))
+