From a32cf498ca953e0ebc0558958977ecd37fc741a5 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 15 Apr 2013 22:24:42 +0200 Subject: host/simu, host/simu/robots/apbirthday: add plate loading simulation --- host/simu/robots/apbirthday/model/cannon.py | 92 +++++++++++++++++++++++++++++ 1 file changed, 92 insertions(+) create mode 100644 host/simu/robots/apbirthday/model/cannon.py (limited to 'host/simu/robots/apbirthday/model/cannon.py') 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)) + -- cgit v1.2.3