From 349546cd507e0a1d3259c7a92aa2c89bb36736e0 Mon Sep 17 00:00:00 2001 From: schodet Date: Mon, 26 May 2003 08:57:42 +0000 Subject: Boulette de merde sur l'inversion des cables. Ajout de GPS. --- 2003/i/buzz/src/busp/Makefile.defs | 3 +- 2003/i/buzz/src/busp/busp.cc | 20 ++++++-- 2003/i/buzz/src/busp/busp.h | 4 ++ 2003/i/buzz/src/busp/busp_gps.cc | 102 +++++++++++++++++++++++++++++++++++++ 2003/i/buzz/src/busp/busp_gps.h | 28 ++++++++++ 2003/i/buzz/src/busp/busp_servo.cc | 1 - 2003/i/buzz/src/busp/busp_servo.h | 2 +- 2003/i/buzz/src/busp/test_busp.cc | 7 +++ 8 files changed, 161 insertions(+), 6 deletions(-) create mode 100644 2003/i/buzz/src/busp/busp_gps.cc create mode 100644 2003/i/buzz/src/busp/busp_gps.h (limited to '2003/i/buzz/src/busp') 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 + +// 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 [ ...]" << endl << "\t" << argv[0] << " actionneurs all " << endl -- cgit v1.2.3