summaryrefslogtreecommitdiff
path: root/2004
diff options
context:
space:
mode:
authorschodet2004-04-25 20:49:40 +0000
committerschodet2004-04-25 20:49:40 +0000
commit0c53c7063dc2764c7d4d1e6ff73f82aaaad9311a (patch)
treeac5c3f6c582027f14a1019544ca18c8cd4116e22 /2004
parent3af9c88516543659002f5303d703f32579b45d6f (diff)
Fichiers manquants.
Diffstat (limited to '2004')
-rw-r--r--2004/i/nono/src/motor/goto_circle.cc117
-rw-r--r--2004/i/nono/src/motor/goto_circle.h52
-rw-r--r--2004/i/nono/src/utils/mathutil.h57
3 files changed, 226 insertions, 0 deletions
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 <iostream> // 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 <cmath>
+
+/// 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