summaryrefslogtreecommitdiff
path: root/2004/i/nono/src/motor/movement_goto.cc
blob: cc720a980cc696e8d5a72bbdb71da4e410c3b9bc (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
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
// movement_goto.cc
// nono - programme du robot 2004. {{{
//
// Copyright (C) 2004 Nicolas Schodet
//
// Robot APB Team/Efrei 2004.
//        Web: http://assos.efrei.fr/robot/
//      Email: robot AT efrei DOT fr
//
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation; either version 2 of the License, or
// (at your option) any later version.
// 
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
//
// }}}
#include "movement_goto.h"
#include "tracker.h"
#include "motor.h"
#include "config/config.h"

MovementGotoParam MovementGoto::param_;

/// Constructeur, charge les param�tres depuis la Config.
MovementGotoParam::MovementGotoParam (void)
    : eps_ (10.0), dist_ (100.0),
      kpl_ (1.0), kpa_ (1.0), kil_ (0.0), kia_ (0.0), is_ (20.0),
      kdl_ (0.0), kda_ (0.0)
{
    // Lit la conf.
    Config rc ("rc/movement/goto");
    while (!rc.eof ())
      {
	if (!(
	      rc.get ("epsilon", eps_) ||
	      rc.get ("distance", dist_) ||
	      rc.get ("kpl", kpl_) ||
	      rc.get ("kpa", kpa_) ||
	      rc.get ("kil", kil_) ||
	      rc.get ("kia", kia_) ||
	      rc.get ("is", is_) ||
	      rc.get ("kdl", kdl_) ||
	      rc.get ("kda", kda_)
	     ))
	    rc.noId ();
      }
}

/// Constructeur.
/// go : objet Goto, detruit dans le destructeur.
MovementGoto::MovementGoto (Goto *go)
    : goto_ (go),
      il_ (0.0), ia_ (0.0), lel_ (0.0), lea_ (0.0)
{
}

/// Destructeur.
MovementGoto::~MovementGoto (void)
{
    delete goto_;
}

/// Initialise le mouvement, appel� juste quand le mouvement est mis en
/// service.
void
MovementGoto::init (const Tracker &t, Asserv &a, Motor &m)
{
    Movement::init (t, a, m);
    goto_->init (t);
}

/// Controlle la vitesse, retourne faux si mouvement termin�.
bool
MovementGoto::control (void)
{
    // R�cup�re la distance et le prochain point.
    double dist, dx, dy;
    if (!goto_->get (*t_, param_.dist_, param_.eps_, dist, dx, dy))
	return false;
std::cout << "movement goto: consign " << dist << ' ' << dx << ' ' << dy <<
    std::endl;
    // Calcule l'erreur.
    double el, ea;
    t_->computeError (dx, dy, el, ea);
    // Pas de marche arri�re.
    if (el < 0.0)
      {
	el = 0.0;
	ea = 2.0 - ea;
      }
std::cout << "movement goto: error " << el << ' ' << ea << std::endl;
    // Calcule les int�grales satur�es.
    il_ += el;
    if (il_ > param_.is_) il_ = param_.is_;
    else if (il_ < -param_.is_) il_ = -param_.is_;
    ia_ += ea;
    if (ia_ > param_.is_) ia_ = param_.is_;
    else if (ia_ < -param_.is_) ia_ = -param_.is_;
    // Commande les moteurs.
    double l = param_.kpl_ * (el + param_.kil_ * il_
			      + param_.kdl_ * (el - lel_));
    double a = param_.kpa_ * (ea + param_.kia_ * ia_
			      + param_.kda_ * (ea - lea_));
std::cout << "movement goto: command " << l << ' ' << a << std::endl;
    m_->speed (l, a, dist);
    // Retiens l'erreur pour la d�riv�e.
    lel_ = el;
    lea_ = ea;
    return true;
}