summaryrefslogtreecommitdiff
path: root/2003/i/buzz/src/busp/busp_gps.cc
diff options
context:
space:
mode:
Diffstat (limited to '2003/i/buzz/src/busp/busp_gps.cc')
-rw-r--r--2003/i/buzz/src/busp/busp_gps.cc102
1 files changed, 102 insertions, 0 deletions
diff --git a/2003/i/buzz/src/busp/busp_gps.cc b/2003/i/buzz/src/busp/busp_gps.cc
new file mode 100644
index 0000000..b2f4ea3
--- /dev/null
+++ b/2003/i/buzz/src/busp/busp_gps.cc
@@ -0,0 +1,102 @@
+// busp_gps.cc
+// buzz - Programme du robot Efrei Robotique I1-I2 2003
+// Copyright (C) 2003 Nicolas Schodet
+//
+#include "busp_gps.h"
+#include "busp.h"
+#include "config/config.h"
+
+#include <iostream>
+
+// Constructeur.
+BuspGPS::BuspGPS ()
+{
+ m_l1 = m_l2 = m_r = 0;
+ // Paramètres par défaut.
+ m_fl1 = 1.0, m_fl2 = 1.0, m_fr = 1.0;
+ m_d = 100.0;
+ // Lecture des paramètres.
+ Config rc ("rc/busp/gps");
+ while (!rc.eof ())
+ {
+ if (rc.isId ("fl1"))
+ {
+ rc.getId ();
+ rc >> m_fl1;
+ cout << "fl1 " << m_fl1 << endl;
+ }
+ else if (rc.isId ("fl2"))
+ {
+ rc.getId ();
+ rc >> m_fl2;
+ cout << "fl2 " << m_fl2 << endl;
+ }
+ else if (rc.isId ("fr"))
+ {
+ rc.getId ();
+ rc >> m_fr;
+ cout << "fr " << m_fr << endl;
+ }
+ else if (rc.isId ("d"))
+ {
+ rc.getId ();
+ rc >> m_d;
+ cout << "d " << m_d << endl;
+ }
+ else rc.noId ();
+ }
+}
+
+// Destructeur.
+BuspGPS::~BuspGPS ()
+{
+}
+
+// Lit les valeurs.
+void
+BuspGPS::read (void)
+{
+ Busp &b = Busp::getInstance ();
+ for (int i = 0; i < 4 && b.read (m_address) != 0xff; ++i);
+ m_l1 = b.read (m_address);
+ m_l2 = b.read (m_address);
+ m_r = b.read (m_address);
+}
+
+// Extrait les valeurs de x et y, true si ok
+bool
+BuspGPS::getXY (int &x, int &y)
+{
+ double l1 = (double) m_l1 * m_fl1;
+ double l2 = (double) m_l2 * m_fl2;
+ double fx, fy;
+ // $x^2 + y^2 = l_1^2$
+ // $(d - x)^2 + y^2 = l_2^2$
+ fx = (l1 * l1 - l2 * l2 + m_d * m_d) / (2 * m_d);
+ fy = l1 * l1 - fx * fx;
+ if (fy <= 0)
+ return false;
+ x = (int) fx;
+ y = (int) fy;
+ return true;
+}
+
+// Extrait les valeurs de r, true si ok.
+bool
+BuspGPS::getR (int &r)
+{
+ r = (int) (m_fr * m_r);
+ return true;
+}
+
+// Sort les informations.
+void
+BuspGPS::dump (void)
+{
+ int x, y, r;
+ cout << "gps " << m_l1 << ' ' << m_l2 << ' ' << m_r << endl;
+ if (getXY (x, y))
+ cout << "gps xy " << x << ' ' << y << endl;
+ if (getR (r))
+ cout << "gps r " << r << endl;
+}