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/bag.py | 11 ++++ host/simu/robots/apbirthday/model/cannon.py | 92 +++++++++++++++++++++++++++++ host/simu/robots/apbirthday/view/bag.py | 3 +- host/simu/robots/apbirthday/view/robot.py | 19 +++++- 4 files changed, 123 insertions(+), 2 deletions(-) create mode 100644 host/simu/robots/apbirthday/model/cannon.py (limited to 'host/simu/robots') 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) -- cgit v1.2.3