summaryrefslogtreecommitdiff
path: root/2004/i/nono/src/motor/movement_reposition.cc
blob: 3ccbd86a86206ea86efcca3d362db13cfac1f41c (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
// movement_reposition.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_reposition.h"
#include "motor.h"
#include "tracker.h"
#include "date/date.h"

/// Constructeur.
/// speed : vitesse (-1..+1).
/// gpio : gpio d'entr�.
/// gpil : gpi gauche.
/// gpir : gpi droite.
/// ana : analog d'entr�e.
/// anal : ana gauche.
/// anar : ana droite.
MovementReposition::MovementReposition (double speed, Gpio &gpio, int gpil,
					int gpir, Analog &ana, int anal, int
					anar)
    : speed_ (speed), gpio_ (gpio), gpil_ (gpil), gpir_ (gpir), ana_ (ana),
      anal_ (anal), anar_ (anar), state_ (0)
{
}

/// Initialise le mouvement, appel� juste quand le mouvement est mis en
/// service.
void
MovementReposition::init (const Tracker &t, Asserv &a, Motor &m)
{
    Movement::init (t, a, m);
    stepDate_ = Date::getInstance ().start ();
}

/// Controlle la vitesse, retourne faux si mouvement termin�.
bool
MovementReposition::control (void)
{
    std::cout << "reposition: " << state_ << std::endl;
    switch (state_)
      {
      case 0:
	  {
	    if (stepDate_ + 5000 < Date::getInstance ().start ())
		return false;
	    double dl = ana_.get (anal_);
	    double dr = ana_.get (anar_);
	    double dm = dl < dr ? dl : dr;
	    std::cout << "reposition: 0 " << dl << ' ' << dr << std::endl;
	    m_->speedLR (dl > 62.0 ? speed_ : -speed_ * 0.5,
			 dr > 62.0 ? speed_ : -speed_ * 0.5,
			 dm - 62.0, M_PI);
	    if (dr <= 62.0 && dl <= 62.0)
	      {
		state_ = 1;
		stepDate_ = Date::getInstance ().start ();
		step1_ = Point (t_->getX (), t_->getY ());
	      }
	  }
	return true;
      case 1:
	  {
	    if (stepDate_ + 5000 < Date::getInstance ().start ())
		return false;
	    double d = 100.0 - step1_.distTo (Point (t_->getX (), t_->getY ()));
	    bool l = gpio_.get (gpil_);
	    bool r = gpio_.get (gpir_);
	    std::cout << "reposition: 1 " << l << ' ' << r << std::endl;
	    if (l && r)
	      {
		state_ = 2;
		return false;
	      }
	    m_->speedLR (!l ? speed_ : -speed_ * 0.05,
			 !r ? speed_ : -speed_ * 0.05,
			 d - 50.0, M_PI);
	  }
	return true;
      case 2:
	  {
	    // XXX: Inutilis�.
	    bool l = gpio_.get (gpil_);
	    bool r = gpio_.get (gpir_);
	    std::cout << "reposition: 2 " << l << ' ' << r << std::endl;
	    if (!l && !r)
		return false;
	    m_->speed (!l ? speed_ * 0.05 : -speed_ * 0.05,
		       !r ? speed_ * 0.05 : -speed_ * 0.05);
	  }
	return true;
      }
    return false;
}