summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--2003/i/buzz/src/busp/Makefile.defs3
-rw-r--r--2003/i/buzz/src/busp/busp.cc20
-rw-r--r--2003/i/buzz/src/busp/busp.h4
-rw-r--r--2003/i/buzz/src/busp/busp_gps.cc102
-rw-r--r--2003/i/buzz/src/busp/busp_gps.h28
-rw-r--r--2003/i/buzz/src/busp/busp_servo.cc1
-rw-r--r--2003/i/buzz/src/busp/busp_servo.h2
-rw-r--r--2003/i/buzz/src/busp/test_busp.cc7
8 files changed, 161 insertions, 6 deletions
diff --git a/2003/i/buzz/src/busp/Makefile.defs b/2003/i/buzz/src/busp/Makefile.defs
index aa3bd09..4ec0fb0 100644
--- a/2003/i/buzz/src/busp/Makefile.defs
+++ b/2003/i/buzz/src/busp/Makefile.defs
@@ -1,5 +1,6 @@
TARGETS += test_busp
test_busp_SOURCES = busp.cc busp_ir.cc busp_servo.cc busp_capteurs.cc \
- busp_actionneurs.cc test_busp.cc erreur.a config.a
+ busp_actionneurs.cc busp_gps.cc test_busp.cc erreur.a \
+ config.a
test_busp: $(test_busp_SOURCES:%.cc=%.o)
diff --git a/2003/i/buzz/src/busp/busp.cc b/2003/i/buzz/src/busp/busp.cc
index 0e90475..d830a30 100644
--- a/2003/i/buzz/src/busp/busp.cc
+++ b/2003/i/buzz/src/busp/busp.cc
@@ -35,6 +35,20 @@ Busp::Busp ()
sigaction (SIGUSR1, &sa, 0); // Il n'y aura pas d'erreur, cf doc.
// Active les interruptions.
sti ();
+ // Remplis le tableau d'inversion des données.
+ // Les fils sont soudés à l'envers.
+ for (int i = 0; i < 256; ++i)
+ {
+ m_inverse[i] =
+ (i << 7) & (1 << 7)
+ | (i << 5) & (1 << 6)
+ | (i << 3) & (1 << 5)
+ | (i << 1) & (1 << 4)
+ | (i >> 1) & (1 << 3)
+ | (i >> 3) & (1 << 2)
+ | (i >> 5) & (1 << 1)
+ | (i >> 7) & (1 << 0);
+ }
}
// Destructeur.
@@ -91,7 +105,7 @@ Busp::write (int addr, int data)
{
struct pbus_io io;
io.addr = addr;
- io.data = data;
+ io.data = m_inverse[data];
ioctl (m_fd, PBUS_WRITE, &io);
cout << "write " << addr << " " << data << endl;
}
@@ -103,6 +117,6 @@ Busp::read (int addr)
struct pbus_io io;
io.addr = addr;
ioctl (m_fd, PBUS_READ, &io);
- cout << "read " << addr << " = " << (int) io.data << endl;
- return io.data;
+ cout << "read " << addr << " = " << (int) m_inverse[io.data] << endl;
+ return m_inverse[io.data];
}
diff --git a/2003/i/buzz/src/busp/busp.h b/2003/i/buzz/src/busp/busp.h
index 3bbabe4..99bcd72 100644
--- a/2003/i/buzz/src/busp/busp.h
+++ b/2003/i/buzz/src/busp/busp.h
@@ -7,6 +7,7 @@
#include "busp_servo.h"
#include "busp_capteurs.h"
#include "busp_actionneurs.h"
+#include "busp_gps.h"
// Gère le busp coté hard.
class Busp
@@ -16,7 +17,9 @@ class Busp
BuspServo m_servo;
BuspCapteurs m_capteurs;
BuspActionneurs m_actionneurs;
+ BuspGPS m_gps;
static Busp *m_instance;
+ unsigned char m_inverse[256];
public:
volatile int irqs;
// Initialise le busp, open (), signal ().
@@ -42,6 +45,7 @@ class Busp
BuspServo &getServo (void) { return m_servo; }
BuspCapteurs &getCapteurs (void) { return m_capteurs; }
BuspActionneurs &getActionneurs (void) { return m_actionneurs; }
+ BuspGPS &getGPS (void) { return m_gps; }
};
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;
+}
diff --git a/2003/i/buzz/src/busp/busp_gps.h b/2003/i/buzz/src/busp/busp_gps.h
new file mode 100644
index 0000000..bc19fe1
--- /dev/null
+++ b/2003/i/buzz/src/busp/busp_gps.h
@@ -0,0 +1,28 @@
+#ifndef busp_gps_h
+#define busp_gps_h
+// busp_gps.h
+// buzz - Programme du robot Efrei Robotique I1-I2 2003
+// Copyright (C) 2003 Nicolas Schodet
+
+class BuspGPS
+{
+ private:
+ static const int m_address = 14;
+ int m_l1, m_l2, m_r;
+ double m_fl1, m_fl2, m_fr, m_d;
+ public:
+ // Constructeur.
+ BuspGPS ();
+ // Destructeur.
+ ~BuspGPS ();
+ // Lit les valeurs du GPS.
+ void read (void);
+ // Extrait les valeurs de x et y, true si ok
+ bool getXY (int &x, int &y);
+ // Extrait les valeurs de r, true si ok.
+ bool getR (int &r);
+ // Sort les informations.
+ void dump (void);
+};
+
+#endif // busp_gps_h
diff --git a/2003/i/buzz/src/busp/busp_servo.cc b/2003/i/buzz/src/busp/busp_servo.cc
index ad95c34..e92f1b2 100644
--- a/2003/i/buzz/src/busp/busp_servo.cc
+++ b/2003/i/buzz/src/busp/busp_servo.cc
@@ -17,7 +17,6 @@ BuspServo::BuspServo ()
for (int i = 0; i < m_nbPositions; ++i)
for (int s = 0; s < m_nbServo; ++s)
{
- m_ppos[i][s] = -1;
rc >> m_ppos[i][s];
}
rc.getEof ();
diff --git a/2003/i/buzz/src/busp/busp_servo.h b/2003/i/buzz/src/busp/busp_servo.h
index 1545034..7cdf76e 100644
--- a/2003/i/buzz/src/busp/busp_servo.h
+++ b/2003/i/buzz/src/busp/busp_servo.h
@@ -8,7 +8,7 @@ class BuspServo
{
static const int m_address = 1;
static const int m_nbServo = 8;
- static const int m_nbPositions = 10;
+ static const int m_nbPositions = 9;
int m_pos[m_nbServo];
int m_ppos[m_nbPositions][m_nbServo];
public:
diff --git a/2003/i/buzz/src/busp/test_busp.cc b/2003/i/buzz/src/busp/test_busp.cc
index 154e83c..3ca65e5 100644
--- a/2003/i/buzz/src/busp/test_busp.cc
+++ b/2003/i/buzz/src/busp/test_busp.cc
@@ -57,6 +57,12 @@ main (int argc, char **argv)
busp.reset ();
cout << "reset" << endl;
}
+ else if (strcmp (argv[1], "gps") == 0)
+ {
+ // Lit une trame sur le GPS.
+ busp.getGPS ().read ();
+ busp.getGPS ().dump ();
+ }
else
{
// Lecture.
@@ -99,6 +105,7 @@ main (int argc, char **argv)
<< "\t" << argv[0] << " reset" << endl
<< "\t" << argv[0] << " irq" << endl
<< "\t" << argv[0] << " capteurs" << endl
+ << "\t" << argv[0] << " gps" << endl
<< "\t" << argv[0] << " actionneurs [<n> <val>...]" << endl
<< "\t" << argv[0] << " actionneurs all <set> <reset>" <<
endl