summaryrefslogtreecommitdiff
path: root/2004/i/nono/src/motor/motor.cc
diff options
context:
space:
mode:
authorschodet2004-02-07 16:55:41 +0000
committerschodet2004-02-07 16:55:41 +0000
commit9fdd6704947174fae292dfc6e14cc1029a7f7a6c (patch)
tree40a4fe775845ececdaf27e82d99d6a06f6b3803e /2004/i/nono/src/motor/motor.cc
parentb673946d76e436c067d24e535b1e0a167b9dfc47 (diff)
Initial revision
Diffstat (limited to '2004/i/nono/src/motor/motor.cc')
-rw-r--r--2004/i/nono/src/motor/motor.cc216
1 files changed, 216 insertions, 0 deletions
diff --git a/2004/i/nono/src/motor/motor.cc b/2004/i/nono/src/motor/motor.cc
new file mode 100644
index 0000000..615511c
--- /dev/null
+++ b/2004/i/nono/src/motor/motor.cc
@@ -0,0 +1,216 @@
+// motor.cc
+// nono - programme du robot 2004. {{{
+//
+// Copyright (C) 2003 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 "motor.h"
+#include "config/config.h"
+
+#include <iostream>
+#include <math.h>
+
+// Pointeur vers l'instance unique.
+Motor *Motor::instance_ = 0;
+
+// Constructeur.
+Motor::Motor (void)
+ : asserv_ (*this),
+ s_ (1.0), vs_ (1.0), as_ (1.0),
+ movement_ (0), maxSpeed_ (0x40), minSpeed_ (0x04)
+{
+ // Mémorise le pointeur d'instance.
+ instance_ = this;
+ // Lit la conf.
+ Config rc ("rc/motor");
+ while (!rc.eof ())
+ {
+ if (rc.isId ("scale"))
+ {
+ rc.getId ();
+ rc >> s_;
+ }
+ else if (rc.isId ("speedscale"))
+ {
+ rc.getId ();
+ rc >> vs_;
+ }
+ else if (rc.isId ("accelscale"))
+ {
+ rc.getId ();
+ rc >> as_;
+ }
+ else if (rc.isId ("maxspeed"))
+ {
+ rc.getId ();
+ rc >> maxSpeed_;
+ }
+ else if (rc.isId ("minspeed"))
+ {
+ rc.getId ();
+ rc >> minSpeed_;
+ }
+ else rc.noId ();
+ }
+ asserv_.reset ();
+}
+
+// Destructeur.
+Motor::~Motor (void)
+{
+ instance_ = 0;
+ delete movement_;
+}
+
+// Appelée lors d'une mise à jour des compteurs.
+void
+Motor::updateCounter (int l, int r)
+{
+ double dL = l * s_;
+ double dR = r * s_;
+ tracker_.update (dL, dR);
+ if (movement_)
+ if (!movement_->control ())
+ {
+ delete movement_;
+ movement_ = 0;
+ asserv_.stop ();
+ }
+}
+
+// Choisi le mouvement en cours.
+void
+Motor::setMovement (Movement *m)
+{
+ delete movement_;
+ movement_ = m;
+ m->init (tracker_, asserv_, *this);
+}
+
+// Teste si les mouvements sont terminés.
+bool
+Motor::end (void)
+{
+ ok ();
+ return !movement_;
+}
+
+// Attend la fin de tout les mouvements.
+void
+Motor::waitEnd (void)
+{
+ do {
+ waitOk ();
+ } while (!end ());
+}
+
+// Teste si les moteurs sont arrétés.
+bool
+Motor::stopped (void)
+{
+ ok ();
+ return tracker_.stopped ();
+}
+
+// Attend que les moteurs soient arrétés.
+void
+Motor::waitStopped (void)
+{
+ do {
+ waitOk ();
+ } while (!stopped ());
+}
+
+// Paramètre la vitesse des moteurs.
+void
+Motor::speed (double l, double r)
+{
+ int vL = (int) (l / vs_);
+ int vR = (int) (r / vs_);
+ // Teste le dépassement de vitesse minimale.
+ while ((l != 0.0 || r != 0.0) && vL > -minSpeed_ && vL < minSpeed_ && vR >
+ -minSpeed_ && vR < minSpeed_)
+ {
+ l *= 1.5;
+ r *= 1.5;
+ vL = (int) (l / vs_);
+ vR = (int) (r / vs_);
+ }
+ // Teste le dépassement de vitesse maximal.
+ if (vL > maxSpeed_)
+ {
+ vL = maxSpeed_;
+ vR = (int) (r / l * vL);
+ }
+ else if (vL < -maxSpeed_)
+ {
+ vL = -maxSpeed_;
+ vR = (int) (r / l * vL);
+ }
+ if (vR > maxSpeed_)
+ {
+ vR = maxSpeed_;
+ vL = (int) (l / r * vR);
+ }
+ else if (vR < -maxSpeed_)
+ {
+ vR = -maxSpeed_;
+ vL = (int) (l / r * vR);
+ }
+ // Envois la commande.
+std::cout << "motor speed " << l << ' ' << r << ' ' << vL << ' ' << vR << std::endl;;
+ asserv_.speed (vL, vR);
+}
+
+// Va vers un point, renvois faux si atteind.
+bool
+Motor::goTo (double x, double y, double eps/*5.0*/)
+{
+ return false;
+}
+
+// Va vers un point, en formant un arc de cercle, renvois faux si atteind.
+bool
+Motor::goToArc (double x, double y, double eps/*5.0*/)
+{
+ double l, r;
+ if (tracker_.computeArcs (x, y, l, r, eps))
+ {
+std::cout << "goto arc " << l << ' ' << r;
+ if (l > 0.0)
+ l = sqrt (2.0 * asserv_.getAccel () * as_ * l);
+ else
+ l = -sqrt (2.0 * asserv_.getAccel () * as_ * -l);
+ if (r > 0.0)
+ r = sqrt (2.0 * asserv_.getAccel () * as_ * r);
+ else
+ r = -sqrt (2.0 * asserv_.getAccel () * as_ * -r);
+std::cout << ' ' << l << ' ' << r << std::endl;;
+ speed (l, r);
+ return true;
+ }
+ else
+ {
+ asserv_.stop ();
+ return false;
+ }
+}
+