From 0c53c7063dc2764c7d4d1e6ff73f82aaaad9311a Mon Sep 17 00:00:00 2001 From: schodet Date: Sun, 25 Apr 2004 20:49:40 +0000 Subject: Fichiers manquants. --- 2004/i/nono/src/motor/goto_circle.cc | 117 +++++++++++++++++++++++++++++++++++ 2004/i/nono/src/motor/goto_circle.h | 52 ++++++++++++++++ 2004/i/nono/src/utils/mathutil.h | 57 +++++++++++++++++ 3 files changed, 226 insertions(+) create mode 100644 2004/i/nono/src/motor/goto_circle.cc create mode 100644 2004/i/nono/src/motor/goto_circle.h create mode 100644 2004/i/nono/src/utils/mathutil.h (limited to '2004/i') diff --git a/2004/i/nono/src/motor/goto_circle.cc b/2004/i/nono/src/motor/goto_circle.cc new file mode 100644 index 0000000..fd81abe --- /dev/null +++ b/2004/i/nono/src/motor/goto_circle.cc @@ -0,0 +1,117 @@ +// goto_circle.cc +// nono - programme du robot 2004. {{{ +// +// Copyright (C) 2004 Nicolas Schodet +// +// Robot APB Team/Efrei 2004. +// Web: http://assos.efrei.fr/robot/ +// Email: robot AT efrei DOT fr +// +// 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. +// +// }}} +#include "goto_circle.h" +#include "utils/mathutil.h" + +#include // XXX + +/// Constructeur. +/// r : rayon (mm). +/// a : angle (rad > 0). +GotoCircle::GotoCircle (double r, double a) + : r_ (r), a_ (a), cx_ (0.0), cy_ (0.0) +{ +} + +/// Initialise le Goto, appelé au début de la trajectoire. +void +GotoCircle::init (const Tracker &t) +{ + // Calcule le centre de rotation. + double x, y, a; + t.getPos (x, y, a); + cx_ = x - sin (a) * r_; + cy_ = y + cos (a) * r_; + // Calcule l'angle de destination. + a_ = a_ + a - M_PI * 0.5; +std::cout << "goto: circle: init " << cx_ << ' ' << cy_ << ' ' << a_ << std::endl; +} + +/// Fournit la distance au point final (mm), le point le plus loin à moins +/// de distmax (mm) et renvois false si le Goto est fini. +/// eps : distance (mm) en dessous de laquelle on considère que la destination +/// est atteinte. +/// distmax : distance (mm) maximale du point. +/// dist : distance (mm) au point final. +/// (x, y) : point intermédiaire (mm). +bool +GotoCircle::get (const Tracker &t, double distmax, double eps, double &dist, + double &x, double &y) +{ + double dx, dy; + dx = t.getX () - cx_; + dy = t.getY () - cy_; + // Calcule l'angle et la distance avec le centre. + double d, a; + toPolair (dx, dy, d, a); +std::cout << "goto: circle: polair " << d << ' ' << a << std::endl; + // Calcule le point intermédiaire. + double aInter, da; + da = angleNorm (a_ - a); + // Corection si angle négatif et que c'est pas cool. + if (r_ > 0.0 && da < 0.0) + { + da = 2.0 * M_PI - da; + } + else if (r_ < 0.0 && da > 0.0) + { + da = -2.0 * M_PI + da; + } +std::cout << "goto: circle: dists " << d << ' ' << r_ << ' ' << distmax << std::endl; + if (d > r_ + distmax || r_ > distmax + d) + { + // Pas d'intersection. +std::cout << "goto: circle: miss" << std::endl; + aInter = a; + } + else + { + // Intersection. + double f, ia; + f = (r_ * r_ - distmax * distmax + d * d) / (2.0 * d); + ia = acos (f / r_); +std::cout << "goto: circle: diff " << da << ' ' << ia << std::endl; + if (da < ia && da > -ia) + { + // On est tout prés, renvois le point destination. + aInter = a_; + std::cout << "goto: circle: near" << std::endl; + } + else + { + std::cout << "goto: circle: far" << std::endl; + // On est loin, renvois un point intermédiaire. + if (r_ > 0.0) + aInter = a + ia; + else + aInter = M_PI + a - ia; + } + } + x = cx_ + r_ * cos (aInter); + y = cy_ + r_ * sin (aInter); + dist = da * r_; + return dist > eps; +} + diff --git a/2004/i/nono/src/motor/goto_circle.h b/2004/i/nono/src/motor/goto_circle.h new file mode 100644 index 0000000..50b576b --- /dev/null +++ b/2004/i/nono/src/motor/goto_circle.h @@ -0,0 +1,52 @@ +#ifndef goto_circle_h +#define goto_circle_h +// goto_circle.h +// nono - programme du robot 2004. {{{ +// +// Copyright (C) 2004 Nicolas Schodet +// +// Robot APB Team/Efrei 2004. +// Web: http://assos.efrei.fr/robot/ +// Email: robot AT efrei DOT fr +// +// 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. +// +// }}} +#include "goto.h" + +/// Classe abstraite d'un fournisseur de point pour MovementGoto. +class GotoCircle : public Goto +{ + double r_, a_; + double cx_, cy_; + public: + /// Constructeur. + /// r : rayon (mm). + /// a : angle (rad > 0). + GotoCircle (double r, double a); + /// Initialise le Goto, appelé au début de la trajectoire. + void init (const Tracker &t); + /// Fournit la distance au point final (mm), le point le plus loin à moins + /// de distmax (mm) et renvois false si le Goto est fini. + /// eps : distance (mm) en dessous de laquelle on considère que la + /// destination est atteinte. + /// distmax : distance (mm) maximale du point. + /// dist : distance (mm) au point final. + /// (x, y) : point intermédiaire (mm). + bool get (const Tracker &t, double distmax, double eps, + double &dist, double &x, double &y); +}; + +#endif // goto_circle_h diff --git a/2004/i/nono/src/utils/mathutil.h b/2004/i/nono/src/utils/mathutil.h new file mode 100644 index 0000000..8d3eee8 --- /dev/null +++ b/2004/i/nono/src/utils/mathutil.h @@ -0,0 +1,57 @@ +#ifndef mathutil_h +#define mathutil_h +// mathutil.h +// nono - programme du robot 2004. {{{ +// +// Copyright (C) 2004 Nicolas Schodet +// +// Robot APB Team/Efrei 2004. +// Web: http://assos.efrei.fr/robot/ +// Email: robot AT efrei DOT fr +// +// 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. +// +// }}} +#include + +/// Normalise l'angle entre -pi et +pi. +double +angleNorm (double a); + +/// Converti en coordonnées polaires. +void +toPolair (double x, double y, double &r, double &a); + +/// Normalise l'angle entre -pi et +pi. +inline double +angleNorm (double a) +{ + while (a > M_PI) + a -= 2.0 * M_PI; + while (a < -M_PI) + a += 2.0 * M_PI; + return a; +} + +/// Converti en coordonnées polaires. +inline void +toPolair (double x, double y, double &r, double &a) +{ + // Rayon. + r = sqrt (x * x + y * y); + a = y > 0 ? acos (x / r) : -acos (x / r); +} + +#endif // mathutil_h -- cgit v1.2.3