// 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; } else if (rc.isId ("fl2")) { rc.getId (); rc >> m_fl2; } else if (rc.isId ("fr")) { rc.getId (); rc >> m_fr; } else if (rc.isId ("d")) { rc.getId (); rc >> m_d; } 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; }