summaryrefslogtreecommitdiff
path: root/2003/i/buzz/src/busp/busp_gps.cc
blob: b2f4ea3f13c3a287353261bd524ea5624de84207 (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
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;
}