From 5c15ce981e9de8432d83714c47790b0f01afdac4 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 29 Apr 2008 09:40:58 +0200 Subject: * host/inter: - added obstacle and distance sensor. --- host/inter/dist_sensor.py | 82 +++++++++++++++++++++++++++++++++++++++++++++++ host/inter/inter.py | 29 +++++++++++++++++ 2 files changed, 111 insertions(+) create mode 100644 host/inter/dist_sensor.py (limited to 'host') diff --git a/host/inter/dist_sensor.py b/host/inter/dist_sensor.py new file mode 100644 index 00000000..54c61e6d --- /dev/null +++ b/host/inter/dist_sensor.py @@ -0,0 +1,82 @@ +# inter - Robot simulation interface. {{{ +# +# Copyright (C) 2008 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. +# +# }}} +"""Distance sensor.""" + +from Tkinter import * +from drawable import * + +from math import pi, cos, sin, sqrt + +class DistSensor (Drawable): + """A distance sensor.""" + + def __init__ (self, onto, pos, angle, range): + Drawable.__init__ (self, onto) + self.pos = pos + self.angle = angle + self.range = range + self.target = (pos[0] + cos (angle) * range, + pos[1] + sin (angle) * range) + self.obstacles = [ ] + self.distance = None + self.hide = False + + def compute (self): + # Could do better by trans_applying until on the same drawable as the + # obstacle. + pos = self.onto.trans_apply (self.pos) + target = self.onto.trans_apply (self.target) + n = ((target[0] - pos[0]) / self.range, + (target[1] - pos[1]) / self.range) + self.distance = None + for o in self.obstacles: + # Does the line intersect with the obstacle? + ao = (o.pos[0] - pos[0], o.pos[1] - pos[1]) + doc = abs (ao[0] * -n[1] + ao[1] * n[0]) + if doc < o.radius: + # Does the segment intersect? + m = ao[0] * n[0] + ao[1] * n[1] + f = sqrt (o.radius ** 2 - doc ** 2) + if m - f > 0 and m - f < self.range: + d = m - f + if self.distance is None or self.distance > d: + self.distance = d + elif m + f > 0 and m + f < self.range: + d = m + f + if self.distance is None or self.distance > d: + self.distance = d + return self.distance + + def draw (self): + self.compute () + self.reset () + if self.hide: + return + if self.distance is None: + self.draw_line (self.pos, self.target, fill = 'blue', arrow = LAST) + else: + inter = (self.pos[0] + cos (self.angle) * self.distance, + self.pos[1] + sin (self.angle) * self.distance) + self.draw_line (self.pos, inter, fill = 'red', arrow = LAST) + self.draw_line (inter, self.target, fill = 'blue', arrow = LAST) diff --git a/host/inter/inter.py b/host/inter/inter.py index a7d95f82..cde1bd73 100644 --- a/host/inter/inter.py +++ b/host/inter/inter.py @@ -30,6 +30,10 @@ from math import pi, cos, sin class Robot (Drawable): """The robot.""" + def __init__ (self, onto): + Drawable.__init__ (self, onto) + self.drawn = [ ] + def draw (self): self.reset () self.trans_rotate (self.angle) @@ -44,6 +48,22 @@ class Robot (Drawable): self.draw_line ((0, +f / 2), (0, -f / 2), fill = axes_fill) self.draw_line ((-wr, f / 2), (+wr, f / 2), fill = axes_fill) self.draw_line ((-wr, -f / 2), (+wr, -f / 2), fill = axes_fill) + for i in self.drawn: + i.draw () + +class Obstacle (Drawable): + """An obstacle.""" + + def __init__ (self, onto, pos, radius): + Drawable.__init__ (self, onto) + self.pos = pos + self.radius = radius + + def draw (self): + self.reset () + self.trans_translate (self.pos) + self.draw_circle ((0, 0), self.radius, fill = '#31aa23') + self.draw_circle ((0, 0), self.radius + 250, outlinestipple = 'gray25') class Table (Drawable): """The table and its elements.""" @@ -128,9 +148,12 @@ class TableView (DrawableCanvas): self.robot = Robot (self.table) self.robot.angle = 0 self.robot.pos = (0, 0) + self.drawn = [ ] def draw (self): self.table.draw () + for i in self.drawn: + i.draw () self.robot.draw () class Arm (Drawable): @@ -263,6 +286,12 @@ if __name__ == '__main__': app = Inter () app.tableview.robot.angle = pi / 3 app.tableview.robot.pos = (700, 700) + if 0: + from dist_sensor import DistSensor + ds = DistSensor (app.tableview.robot, (150, -127), -pi / 12, 800) + app.tableview.robot.drawn.append (ds) + app.tableview.drawn.append (Obstacle (app.tableview, (1300, 1200), 150)) + ds.obstacles = app.tableview.drawn app.actuatorview.arm.angle = pi/6 app.actuatorview.rear.traps[0].pos = 1 app.actuatorview.rear.traps[1].pos = 0 -- cgit v1.2.3