summaryrefslogtreecommitdiff
path: root/host/simu/robots/robospierre/model/clamp.py
diff options
context:
space:
mode:
authorNicolas Schodet2011-04-30 20:45:16 +0200
committerNicolas Schodet2011-04-30 20:45:46 +0200
commit4909f63bea2598489a5895f9bbe1e2622093caa1 (patch)
tree302ecbddc392ae78121cc5f9e166c067f576dfe5 /host/simu/robots/robospierre/model/clamp.py
parentecdce887d87139db2e869e72918b4b5be8da5e67 (diff)
digital/io/tools, host/simu: add robospierre
Diffstat (limited to 'host/simu/robots/robospierre/model/clamp.py')
-rw-r--r--host/simu/robots/robospierre/model/clamp.py172
1 files changed, 172 insertions, 0 deletions
diff --git a/host/simu/robots/robospierre/model/clamp.py b/host/simu/robots/robospierre/model/clamp.py
new file mode 100644
index 00000000..ff27a7ea
--- /dev/null
+++ b/host/simu/robots/robospierre/model/clamp.py
@@ -0,0 +1,172 @@
+# simu - Robot simulation. {{{
+#
+# Copyright (C) 2011 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.
+#
+# }}}
+"""Robospierre clamp."""
+from utils.observable import Observable
+from simu.utils.trans_matrix import TransMatrix
+from math import pi, cos, sin
+
+class Slot:
+ """Slot which can contain a pawn."""
+
+ def __init__ (self, x, y, z, side):
+ self.x = x
+ self.y = y
+ self.z = z
+ self.side = side
+ self.pawn = None
+
+class Clamp (Observable):
+
+ ELEVATION_STROKE = 120.0
+ ELEVATION_MOTOR_STROKE = 120.0 * 5.0 / 6.0
+
+ ROTATION_STROKE = pi
+ ROTATION_MOTOR_STROKE = pi * 115.0 / 12.0
+
+ CLAMPING_STROKE = 10
+ CLAMPING_MOTOR_STROKE = pi
+
+ BAY_OFFSET = 150
+ BAY_ZOFFSET = 60
+
+ SLOT_FRONT_BOTTOM = 0
+ SLOT_FRONT_MIDDLE = 1
+ SLOT_FRONT_TOP = 2
+ SLOT_BACK_BOTTOM = 3
+ SLOT_BACK_MIDDLE = 4
+ SLOT_BACK_TOP = 5
+ SLOT_SIDE = 6
+
+ def __init__ (self, table, robot_position, elevation_motor,
+ rotation_motor, clamping_motor):
+ Observable.__init__ (self)
+ self.table = table
+ self.robot_position = robot_position
+ self.elevation_motor = elevation_motor
+ self.rotation_motor = rotation_motor
+ self.clamping_motor = clamping_motor
+ self.slots = (
+ Slot (self.BAY_OFFSET, 0, 0 * self.BAY_ZOFFSET, 0),
+ Slot (self.BAY_OFFSET, 0, 1 * self.BAY_ZOFFSET, 0),
+ Slot (self.BAY_OFFSET, 0, 2 * self.BAY_ZOFFSET, 0),
+ Slot (-self.BAY_OFFSET, 0, 0 * self.BAY_ZOFFSET, 1),
+ Slot (-self.BAY_OFFSET, 0, 1 * self.BAY_ZOFFSET, 1),
+ Slot (-self.BAY_OFFSET, 0, 2 * self.BAY_ZOFFSET, 1),
+ Slot (0, self.BAY_OFFSET, 2 * self.BAY_ZOFFSET, None))
+ self.load = None
+ self.robot_position.register (self.__robot_position_notified)
+ self.elevation_motor.register (self.__elevation_notified)
+ self.rotation_motor.register (self.__rotation_notified)
+ self.clamping_motor.register (self.__clamping_notified)
+
+ def __robot_position_notified (self):
+ # Update bottom slots.
+ changed = False
+ for sloti in (self.SLOT_FRONT_BOTTOM, self.SLOT_BACK_BOTTOM):
+ slot = self.slots[sloti]
+ if slot.pawn is None:
+ p = self.__get_floor_elements (slot.side)
+ if p is not None:
+ slot.pawn = p
+ p.pos = None
+ p.notify ()
+ changed = True
+ if changed:
+ self.notify ()
+
+ def __elevation_notified (self):
+ if self.elevation_motor.angle is None:
+ self.elevation = None
+ else:
+ # Update elevation.
+ self.elevation = (self.elevation_motor.angle
+ * self.ELEVATION_STROKE / self.ELEVATION_MOTOR_STROKE)
+ self.notify ()
+
+ def __rotation_notified (self):
+ if self.rotation_motor.angle is None:
+ self.rotation = None
+ else:
+ # Update rotation.
+ self.rotation = (self.rotation_motor.angle * self.ROTATION_STROKE
+ / self.ROTATION_MOTOR_STROKE)
+ self.notify ()
+
+ def __clamping_notified (self):
+ if self.clamping_motor.angle is None:
+ self.clamping = None
+ else:
+ # Update clamping.
+ self.clamping = (self.clamping_motor.angle * self.CLAMPING_STROKE
+ / self.CLAMPING_MOTOR_STROKE)
+ if self.clamping == 0 and self.load is None:
+ # Load an element.
+ slot = self.__get_clamp_slot ()
+ if slot:
+ self.load, slot.pawn = slot.pawn, self.load
+ elif self.clamping == self.CLAMPING_STROKE \
+ and self.load is not None:
+ # Unload an element.
+ slot = self.__get_clamp_slot ()
+ if slot and slot.pawn is None:
+ self.load, slot.pawn = slot.pawn, self.load
+ self.notify ()
+
+ def __get_floor_elements (self, side):
+ """Return an elements in front (side = 0) or in back (side = 1) of the
+ robot, on the floor."""
+ if self.robot_position.pos is None:
+ return None
+ # Matrix to transform an obstacle position into robot coordinates.
+ m = TransMatrix ()
+ m.rotate (-self.robot_position.angle)
+ m.translate ((-self.robot_position.pos[0],
+ -self.robot_position.pos[1]))
+ # Look up elements.
+ xoffset = (self.BAY_OFFSET, -self.BAY_OFFSET)[side]
+ xmargin = 20
+ ymargin = 50
+ for o in self.table.obstacles:
+ if o.level == 1 and o.pos is not None:
+ pos = m.apply (o.pos)
+ if (pos[0] > xoffset - xmargin
+ and pos[0] < xoffset + xmargin
+ and pos[1] > -ymargin and pos[1] < ymargin):
+ return o
+ return None
+
+ def __get_clamp_slot (self):
+ """Return the slot in which the clamp is."""
+ if self.rotation is None or self.elevation is None:
+ return None
+ margin = 10
+ x = cos (self.rotation) * self.BAY_OFFSET
+ y = sin (self.rotation) * self.BAY_OFFSET
+ for slot in self.slots:
+ if abs (slot.z - self.elevation) < margin \
+ and abs (slot.x - x) < margin \
+ and abs (slot.y - y) < margin:
+ return slot
+ return None
+