summaryrefslogtreecommitdiff
path: root/2004/i/nono/src/motor/movement_reposition.cc
diff options
context:
space:
mode:
Diffstat (limited to '2004/i/nono/src/motor/movement_reposition.cc')
-rw-r--r--2004/i/nono/src/motor/movement_reposition.cc114
1 files changed, 114 insertions, 0 deletions
diff --git a/2004/i/nono/src/motor/movement_reposition.cc b/2004/i/nono/src/motor/movement_reposition.cc
new file mode 100644
index 0000000..3ccbd86
--- /dev/null
+++ b/2004/i/nono/src/motor/movement_reposition.cc
@@ -0,0 +1,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;
+}
+