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 --- digital/ai/tools/test_simu_control_apbirthday.py | 6 +- host/simu/model/rectangular_obstacle.py | 11 +++ host/simu/model/table_eurobot2013.py | 7 +- 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 ++++- host/simu/view/table_eurobot2013.py | 21 ++---- 8 files changed, 145 insertions(+), 25 deletions(-) create mode 100644 host/simu/robots/apbirthday/model/cannon.py diff --git a/digital/ai/tools/test_simu_control_apbirthday.py b/digital/ai/tools/test_simu_control_apbirthday.py index d88d5f7b..6260ffa0 100644 --- a/digital/ai/tools/test_simu_control_apbirthday.py +++ b/digital/ai/tools/test_simu_control_apbirthday.py @@ -37,8 +37,8 @@ class TestSimuControl (TestSimu): self.asserv = self.robots[0].asserv self.robot_model = self.robots[0].model self.io.output (io_hub.apbirthday.output_mask ( - 'cake_arm_in', 'cake_push_far_in', 'cake_push_near_in'), - 'toggle') + 'cake_arm_in', 'cake_push_far_in', 'cake_push_near_in', + 'cherry_plate_down'), 'toggle') def create_widgets (self): TestSimu.create_widgets (self) @@ -59,6 +59,8 @@ class TestSimuControl (TestSimu): out_button ('Arm in/out', 'cake_arm_in', 'cake_arm_out') out_button ('Push far in/out', 'cake_push_far_in', 'cake_push_far_out') out_button ('Push near in/out', 'cake_push_near_in', 'cake_push_near_out') + out_button ('Plate arm up/down', 'cherry_plate_up', 'cherry_plate_down') + out_button ('Plate clamp', 'cherry_plate_clamp') self.backward_var = IntVar () self.backward_button = Checkbutton (self.control_frame, text = 'Backward', variable = self.backward_var) diff --git a/host/simu/model/rectangular_obstacle.py b/host/simu/model/rectangular_obstacle.py index 71cfdc8b..8f574f01 100644 --- a/host/simu/model/rectangular_obstacle.py +++ b/host/simu/model/rectangular_obstacle.py @@ -61,3 +61,14 @@ class RectangularObstacle (Observable): found = i return found + def inside (self, a): + """If A is inside obstacle, return True.""" + # Map point in obstacle coordinates. + u = vector.polar (self.angle, 1) + o = vector (self.pos) + a = vector (a) + oa = a - o + x = oa * u / (.5 * self.dim[0]) + y = oa * u.normal () / (.5 * self.dim[1]) + return x > -1 and x < 1 and y > -1 and y < 1 + diff --git a/host/simu/model/table_eurobot2013.py b/host/simu/model/table_eurobot2013.py index 8e5e83fc..d73b0564 100644 --- a/host/simu/model/table_eurobot2013.py +++ b/host/simu/model/table_eurobot2013.py @@ -76,11 +76,11 @@ class Table (simu.model.table.Table): add_glass ((1200, 1050)) # Cherries. self.plates = [ ] - self.cherries = [ ] def add_plate (pos, color): plate = RectangularObstacle ((170, 170), 0) plate.pos = pos plate.angle = 0 + plate.cherries = [ ] self.plates.append (plate) cpos = ((-42, -42), (-42, 0), (-42, +42), (0, -21), (0, 21), (42, -42), (42, 0), (42, +42)) @@ -88,9 +88,9 @@ class Table (simu.model.table.Table): random.shuffle (ccol) for p, c in zip (cpos, ccol): cherry = RoundObstacle (20, 0) - cherry.pos = (pos[0] + p[0], pos[1] + p[1]) + cherry.pos = p cherry.color = c - self.cherries.append (cherry) + plate.cherries.append (cherry) for py in (250, 600, 1000, 1400, 1750): add_plate ((200, py), False) add_plate ((3000 - 200, py), True) @@ -114,6 +114,5 @@ class Table (simu.model.table.Table): self.obstacles += self.candles self.obstacles += self.glasses self.obstacles += self.plates - self.obstacles += self.cherries self.obstacles += self.gifts 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) diff --git a/host/simu/view/table_eurobot2013.py b/host/simu/view/table_eurobot2013.py index 889fe369..6acd847d 100644 --- a/host/simu/view/table_eurobot2013.py +++ b/host/simu/view/table_eurobot2013.py @@ -79,26 +79,15 @@ class Plate (Drawable): self.reset () if self.model.pos: self.trans_translate (self.model.pos) + self.trans_rotate (self.model.angle) w, h = self.model.dim w, h = w/2, h/2 self.draw_rectangle ((-w, -h), (w, h), fill = PINK) self.draw_rectangle ((-w + 22, -h + 22), (w - 22, h - 22), fill = PINK) - Drawable.draw (self) - -class Cherry (Drawable): - - def __init__ (self, onto, model): - Drawable.__init__ (self, onto) - self.model = model - self.model.register (self.update) - - def draw (self): - self.reset () - model = self.model - if model.pos: - self.draw_circle (model.pos, model.radius, - fill = colors[model.color]) + for c in self.model.cherries: + if c.pos: + self.draw_circle (c.pos, c.radius, fill = colors[c.color]) Drawable.draw (self) class Gift (Drawable): @@ -133,8 +122,6 @@ class Table (Drawable): Glass (self, e) for e in self.model.plates: Plate (self, e) - for e in self.model.cherries: - Cherry (self, e) for e in self.model.gifts: Gift (self, e) -- cgit v1.2.3