summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
-rw-r--r--digital/io/tools/test_simu.py23
-rw-r--r--host/simu/model/distance_sensor.py3
-rw-r--r--host/simu/model/distance_sensor_sharps.py64
-rw-r--r--host/simu/model/round_obstacle.py6
-rw-r--r--host/simu/robots/giboulee/model/bag.py16
-rw-r--r--host/simu/robots/giboulee/view/bag.py3
-rw-r--r--host/simu/view/distance_sensor.py52
-rw-r--r--host/simu/view/round_obstacle.py49
-rw-r--r--host/simu/view/table_eurobot2008.py3
9 files changed, 213 insertions, 6 deletions
diff --git a/digital/io/tools/test_simu.py b/digital/io/tools/test_simu.py
index 158b006e..16114c5b 100644
--- a/digital/io/tools/test_simu.py
+++ b/digital/io/tools/test_simu.py
@@ -32,7 +32,12 @@ import io
import io.init
from proto.popen_io import PopenIO
+import simu.model.table as table_model
import simu.view.table_eurobot2008 as table
+
+import simu.model.round_obstacle as obstacle_model
+import simu.view.round_obstacle as obstacle_view
+
import simu.robots.giboulee.link.bag as robot_link
import simu.robots.giboulee.model.bag as robot_model
import simu.robots.giboulee.view.bag as robot_view
@@ -66,14 +71,19 @@ class TestSimu (InterNode):
self.io.async = True
self.tk.createfilehandler (self.io, READABLE, self.io_read)
# Add table.
- self.table = table.Table (self.table_view)
+ self.table_model = table_model.Table ()
+ self.table = table.Table (self.table_view, self.table_model)
+ self.obstacle = obstacle_model.RoundObstacle (150)
+ self.table_model.obstacles.append (self.obstacle)
+ self.obstacle_view = obstacle_view.RoundObstacle (self.table,
+ self.obstacle)
+ self.table_view.bind ('<2>', self.place_obstacle)
# Add robot.
self.robot_link = robot_link.Bag (self.node)
- self.robot_model = robot_model.Bag (self.robot_link)
+ self.robot_model = robot_model.Bag (self.node, self.table_model,
+ self.robot_link)
self.robot_view = robot_view.Bag (self.table, self.actuator_view,
self.sensor_frame, self.robot_model)
- for adc in self.robot_link.io.adc:
- adc.value = 0
# Color switch.
self.robot_model.color_switch.register (self.change_color)
self.change_color ()
@@ -104,6 +114,11 @@ class TestSimu (InterNode):
i = self.robot_model.color_switch.state
self.asserv.set_simu_pos (*self.robot_start_pos[i]);
+ def place_obstacle (self, ev):
+ pos = self.table_view.screen_coord ((ev.x, ev.y))
+ self.obstacle.pos = pos
+ self.obstacle.notify ()
+
if __name__ == '__main__':
app = TestSimu (('../../asserv/src/asserv/asserv.host', '-m', 'giboulee'),
('../src/io.host'))
diff --git a/host/simu/model/distance_sensor.py b/host/simu/model/distance_sensor.py
index 0fa34d15..3dc83f45 100644
--- a/host/simu/model/distance_sensor.py
+++ b/host/simu/model/distance_sensor.py
@@ -43,6 +43,9 @@ class DistanceSensor:
pos, target = self.pos, self.target
m = TransMatrix ()
for i in self.into:
+ if i.pos is None:
+ self.distance = None
+ return
m.rotate (i.angle)
m.translate (i.pos)
pos, target = m.apply (pos, target)
diff --git a/host/simu/model/distance_sensor_sharps.py b/host/simu/model/distance_sensor_sharps.py
new file mode 100644
index 00000000..b4cf1e33
--- /dev/null
+++ b/host/simu/model/distance_sensor_sharps.py
@@ -0,0 +1,64 @@
+# simu - Robot simulation. {{{
+#
+# Copyright (C) 2009 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.
+#
+# }}}
+from utils.observable import Observable
+from simu.model.distance_sensor import DistanceSensor
+
+class DistanceSensorSharps (Observable, DistanceSensor):
+
+ RANGE = 800
+ NEAR = 70
+ A = 0.36
+ B = 177.26
+
+ def __init__ (self, link, scheduler, table, pos, angle, into = None,
+ level = 0):
+ Observable.__init__ (self)
+ DistanceSensor.__init__ (self, table, pos, angle, self.RANGE, into,
+ level)
+ self.link = link
+ self.scheduler = scheduler
+ self.register (self.__update)
+ self.evaluate ()
+
+ def evaluate (self):
+ # Compute real distance.
+ DistanceSensor.evaluate (self)
+ # Convert to sharps voltage.
+ d = self.distance
+ if self.distance is None:
+ d = self.RANGE
+ if d < self.NEAR:
+ self.value = d / self.NEAR * (self.A + self.B / self.NEAR)
+ else:
+ self.value = self.A + self.B / d
+ # Update observers.
+ self.notify ()
+ # Prepare next update.
+ self.scheduler.schedule (self.scheduler.date
+ + int (self.scheduler.tick * 0.040), self.evaluate)
+
+ def __update (self):
+ self.link.value = self.value
+ self.link.notify ()
+
diff --git a/host/simu/model/round_obstacle.py b/host/simu/model/round_obstacle.py
index fc0cfc6a..6658ed19 100644
--- a/host/simu/model/round_obstacle.py
+++ b/host/simu/model/round_obstacle.py
@@ -23,10 +23,12 @@
# }}}
"""Obstacle with a round shape."""
from math import pi, cos, sin, sqrt
+from utils.observable import Observable
-class RoundObstacle:
+class RoundObstacle (Observable):
def __init__ (self, radius, level = 0):
+ Observable.__init__ (self)
self.pos = None
self.radius = radius
self.level = level
@@ -34,6 +36,8 @@ class RoundObstacle:
def intersect (self, a, b):
"""If the segment [AB] intersects the obstacle, return distance from a
to intersection point, else, return None."""
+ if self.pos is None:
+ return None
ab = sqrt ((b[0] - a[0]) ** 2 + (b[1] - a[1]) ** 2) # distance AB.
n = ((b[0] - a[0]) / ab, (b[1] - a[1]) / ab) # vector of length 1.
o = self.pos # obstacle center.
diff --git a/host/simu/robots/giboulee/model/bag.py b/host/simu/robots/giboulee/model/bag.py
index fadccabe..03f85d4e 100644
--- a/host/simu/robots/giboulee/model/bag.py
+++ b/host/simu/robots/giboulee/model/bag.py
@@ -24,15 +24,29 @@
"""Giboulee bag of models."""
from simu.model.switch import Switch
from simu.model.position import Position
+from simu.model.distance_sensor_sharps import DistanceSensorSharps
from simu.robots.giboulee.model.arm import Arm
from simu.robots.giboulee.model.sorter import Sorter
+from math import pi
class Bag:
- def __init__ (self, link_bag):
+ def __init__ (self, scheduler, table, link_bag):
self.jack = Switch (link_bag.io.jack)
self.color_switch = Switch (link_bag.io.color_switch)
self.position = Position (link_bag.asserv.position)
self.arm = Arm (link_bag.asserv.aux[0])
self.sorter = Sorter (link_bag.io.servo[0:5], link_bag.io.servo[5])
+ self.distance_sensor = [
+ DistanceSensorSharps (link_bag.io.adc[0], scheduler, table,
+ (150, 127), 0, (self.position, )),
+ DistanceSensorSharps (link_bag.io.adc[1], scheduler, table,
+ (150, 0), 0, (self.position, )),
+ DistanceSensorSharps (link_bag.io.adc[2], scheduler, table,
+ (150, -127), 0, (self.position, )),
+ DistanceSensorSharps (link_bag.io.adc[3], scheduler, table,
+ (-70, 100), pi, (self.position, )),
+ DistanceSensorSharps (link_bag.io.adc[4], scheduler, table,
+ (-70, -100), pi, (self.position, )),
+ ]
diff --git a/host/simu/robots/giboulee/view/bag.py b/host/simu/robots/giboulee/view/bag.py
index 709d775f..bb48a6a5 100644
--- a/host/simu/robots/giboulee/view/bag.py
+++ b/host/simu/robots/giboulee/view/bag.py
@@ -23,6 +23,7 @@
# }}}
"""Giboulee bag of views."""
from simu.view.switch import Switch
+from simu.view.distance_sensor import DistanceSensor
from simu.robots.giboulee.view.robot import Robot
from simu.robots.giboulee.view.arm import Arm
from simu.robots.giboulee.view.sorter import Sorter
@@ -38,4 +39,6 @@ class Bag:
model_bag.arm)
self.sorter = Sorter (actuator_view.add_view (Sorter.width,
Sorter.height), model_bag.sorter)
+ self.distance_sensor = [DistanceSensor (self.robot, ds)
+ for ds in model_bag.distance_sensor]
diff --git a/host/simu/view/distance_sensor.py b/host/simu/view/distance_sensor.py
new file mode 100644
index 00000000..3fa7dbac
--- /dev/null
+++ b/host/simu/view/distance_sensor.py
@@ -0,0 +1,52 @@
+# simu - Robot simulation. {{{
+#
+# Copyright (C) 2009 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.
+#
+# }}}
+"""Generic distance sensor model."""
+from simu.inter.drawable import Drawable
+from math import sin, cos
+
+class DistanceSensor (Drawable):
+
+ def __init__ (self, onto, model):
+ Drawable.__init__ (self, onto)
+ self.model = model
+ self.model.register (self.__notified)
+ self.__notified ()
+
+ def __notified (self):
+ self.distance = self.model.distance
+ self.update ()
+
+ def draw (self):
+ self.reset ()
+ if self.distance is None:
+ self.draw_line (self.model.pos, self.model.target, fill = 'blue',
+ arrow = 'last')
+ else:
+ inter = (self.model.pos[0] + cos (self.model.angle) * self.distance,
+ self.model.pos[1] + sin (self.model.angle) * self.distance)
+ self.draw_line (self.model.pos, inter, fill = 'red',
+ arrow = 'last')
+ self.draw_line (inter, self.model.target, fill = 'blue',
+ arrow = 'last')
+ Drawable.draw (self)
diff --git a/host/simu/view/round_obstacle.py b/host/simu/view/round_obstacle.py
new file mode 100644
index 00000000..3e218029
--- /dev/null
+++ b/host/simu/view/round_obstacle.py
@@ -0,0 +1,49 @@
+# simu - Robot simulation. {{{
+#
+# Copyright (C) 2009 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.
+#
+# }}}
+"""Obstacle with a round shape."""
+from simu.inter.drawable import Drawable
+
+class RoundObstacle (Drawable):
+
+ MARGIN = 250
+
+ def __init__ (self, onto, model):
+ Drawable.__init__ (self, onto)
+ self.model = model
+ self.model.register (self.__notified)
+ self.__notified ()
+
+ def __notified (self):
+ self.pos = self.model.pos
+ self.update ()
+
+ def draw (self):
+ self.reset ()
+ if self.pos:
+ self.trans_translate (self.pos)
+ self.draw_circle ((0, 0), self.model.radius, fill = '#31aa23')
+ self.draw_circle ((0, 0), self.model.radius + self.MARGIN,
+ outlinestipple = 'gray25')
+ Drawable.draw (self)
+
diff --git a/host/simu/view/table_eurobot2008.py b/host/simu/view/table_eurobot2008.py
index b956101a..afe8bc99 100644
--- a/host/simu/view/table_eurobot2008.py
+++ b/host/simu/view/table_eurobot2008.py
@@ -27,6 +27,9 @@ from simu.inter.drawable import Drawable
class Table (Drawable):
"""The table and its elements."""
+ def __init__ (self, onto, table_model):
+ Drawable.__init__ (self, onto)
+
def draw (self):
# Redraw.
self.reset ()