summaryrefslogtreecommitdiff
path: root/2004
diff options
context:
space:
mode:
authorschodet2004-05-02 04:32:30 +0000
committerschodet2004-05-02 04:32:30 +0000
commitf6702333c361551314d3988946bc4a37d987d3a8 (patch)
treee9622355a29f8f7dd5711e50e474eee4ab273bf5 /2004
parentca55222ed88ced5ded14049488fabbf62143174b (diff)
Oula, il date le dernier commit, voir les diff.
Diffstat (limited to '2004')
-rw-r--r--2004/i/nono/src/motor/Makefile.defs13
-rw-r--r--2004/i/nono/src/motor/asserv.cc48
-rw-r--r--2004/i/nono/src/motor/asserv.h14
-rw-r--r--2004/i/nono/src/motor/goto_circle.h2
-rw-r--r--2004/i/nono/src/motor/goto_hermite.cc140
-rw-r--r--2004/i/nono/src/motor/goto_hermite.h67
-rw-r--r--2004/i/nono/src/motor/goto_simple.h2
-rw-r--r--2004/i/nono/src/motor/motor.cc68
-rw-r--r--2004/i/nono/src/motor/motor.h15
-rw-r--r--2004/i/nono/src/motor/movement_basic.cc1
-rw-r--r--2004/i/nono/src/motor/movement_round_cruise.cc42
-rw-r--r--2004/i/nono/src/motor/movement_round_cruise.h42
-rw-r--r--2004/i/nono/src/motor/movement_types.h2
-rw-r--r--2004/i/nono/src/motor/test_goto_hermite.cc37
-rw-r--r--2004/i/nono/src/motor/test_tracker.cc2
-rw-r--r--2004/i/nono/src/motor/tracker.cc12
-rw-r--r--2004/i/nono/src/motor/tracker.h2
17 files changed, 455 insertions, 54 deletions
diff --git a/2004/i/nono/src/motor/Makefile.defs b/2004/i/nono/src/motor/Makefile.defs
index 84ea47d..033783f 100644
--- a/2004/i/nono/src/motor/Makefile.defs
+++ b/2004/i/nono/src/motor/Makefile.defs
@@ -1,4 +1,4 @@
-TARGETS += test_motor test_asserv test_tracker
+TARGETS += test_motor test_asserv test_tracker test_goto_hermite
LIBS += motor.a
test_motor_SOURCES = test_motor.cc motor.a serial.a erreur.a config.a date.a \
utils.a logger.a
@@ -6,9 +6,12 @@ test_asserv_SOURCES = test_asserv.cc motor.a serial.a erreur.a config.a \
date.a utils.a logger.a
test_tracker_SOURCES = test_tracker.cc motor.a serial.a erreur.a config.a \
date.a utils.a logger.a
-motor_a_SOURCES = asserv.cc tracker.cc motor.cc movement_basic.cc \
- movement_goto.cc movement_rotation.cc goto_simple.cc \
- goto_circle.cc
+motor_a_SOURCES = asserv.cc tracker.cc motor.cc \
+ movement_basic.cc movement_goto.cc movement_rotation.cc \
+ movement_round_cruise.cc \
+ goto_simple.cc goto_circle.cc
+test_goto_hermite_SOURCES = goto_hermite.cc test_goto_hermite.cc tracker.cc \
+ config.a logger.a erreur.a
test_motor: $(test_motor_SOURCES:%.cc=%.o)
@@ -16,6 +19,8 @@ test_asserv: $(test_asserv_SOURCES:%.cc=%.o)
test_tracker: $(test_tracker_SOURCES:%.cc=%.o)
+test_goto_hermite: $(test_goto_hermite_SOURCES:%.cc=%.o)
+
motor.a: ${motor_a_SOURCES:%.cc=motor.a(%.o)}
clean_motor:
diff --git a/2004/i/nono/src/motor/asserv.cc b/2004/i/nono/src/motor/asserv.cc
index 8a2a579..c806479 100644
--- a/2004/i/nono/src/motor/asserv.cc
+++ b/2004/i/nono/src/motor/asserv.cc
@@ -31,6 +31,7 @@
Asserv::Asserv (AsservTracker &asservTracker)
: ttyspeed_ (0), accel_ (-1), kp_ (-1), ki_ (-1), kd_ (-1),
statMotor_ (-1), counter_ (false), posAsserv_ (false), asserv_ (false),
+ zeroEps_ (0),
noSetParam_ (false),
inBufSize_ (64), inBufPos_ (0), inBuf_ (0),
firstCounter_ (true),
@@ -52,6 +53,7 @@ Asserv::Asserv (AsservTracker &asservTracker)
rc.get ("stats", statMotor_) ||
rc.get ("counter", counter_) ||
rc.get ("posasserv", posAsserv_) ||
+ rc.get ("zeroeps", zeroEps_) ||
rc.get ("buffer", inBufSize_)
))
rc.noId ();
@@ -98,7 +100,7 @@ Asserv::setParam (void)
if (ki_ != -1)
setKi (ki_);
if (kd_ != -1)
- setKi (kd_);
+ setKd (kd_);
if (statMotor_ != -1)
setStatMotor (statMotor_);
if (posAsserv_)
@@ -135,7 +137,7 @@ Asserv::ok (void)
void
Asserv::waitOk (void)
{
- while (!ok ()) serial_.wait (100);
+ while (!ok ()) serial_.wait (50);
}
/// Lit et traite les messages de la cartes.
@@ -221,6 +223,20 @@ Asserv::setAsserv (bool fl/*true*/)
asserv_ = fl;
}
+/// Get factor to deduce speed scale from scale.
+double
+Asserv::getSpeedFactor (void) const
+{
+ return 1.0 / 0.004;
+}
+
+/// Get factor to deduce accel scale from scale.
+double
+Asserv::getAccelFactor (void) const
+{
+ return getSpeedFactor () / 0.004 / accel_;
+}
+
/// Envoie un message.
void
Asserv::send (char com)
@@ -366,14 +382,20 @@ Asserv::handleStatMotor (void)
if (inBufPos_ != 2 + 1 + 2 + 1 + 4 + 1 + 4 + 1)
{
// Mauvaise transmission.
- log_ (Log::warning) << "stat motor error" << std::endl;
+ log_ (Log::warning) << "stat motor error (n)" << std::endl;
return;
}
char side = inBuf_[2];
int vacc = hexSignedChar2int (inBuf_ + 2 + 1);
int e = hexSignedShort2int (inBuf_ + 2 + 1 + 2 + 1);
int pwm = hexSignedShort2int (inBuf_ + 2 + 1 + 2 + 1 + 4 + 1);
- log_ (Log::verydebug) << "stat motor " << side << ' ' << vacc << ' ' << e
+ if (vacc == hexInvalid || e == hexInvalid || pwm == hexInvalid)
+ {
+ // Mauvaise transmission.
+ log_ (Log::warning) << "stat motor error (?)" << std::endl;
+ return;
+ }
+ log_ (Log::debug) << "stat motor " << side << ' ' << vacc << ' ' << e
<< ' ' << pwm << std::endl;
}
@@ -384,11 +406,17 @@ Asserv::handleCounter (void)
if (inBufPos_ != 2 + 4 + 1 + 4 + 1)
{
// Mauvaise transmission.
- log_ (Log::warning) << "counter error" << std::endl;
+ log_ (Log::warning) << "counter error (n)" << std::endl;
return;
}
int l = hexSignedShort2int (inBuf_ + 2);
int r = hexSignedShort2int (inBuf_ + 2 + 4 + 1);
+ if (l == hexInvalid || r == hexInvalid)
+ {
+ // Mauvaise transmission.
+ log_ (Log::warning) << "counter error (?)" << std::endl;
+ return;
+ }
if (firstCounter_)
{
// Première valeur ignorée.
@@ -410,9 +438,13 @@ Asserv::handleCounter (void)
log_ (Log::verydebug) << "counter after " << countLeft_ << ' ' <<
countRight_ << ' ' << l << ' ' << r << std::endl;
// Met à jour le tracker.
- asservTracker_.updateCounter (l - countLeft_, r - countRight_);
- log_ (Log::verydebug) << "tracker update " << l - countLeft_ << ' ' << r -
- countRight_ << std::endl;
+ int ldiff = l - countLeft_;
+ int rdiff = r - countRight_;
+ bool zero = rdiff > -zeroEps_ && rdiff < zeroEps_ && ldiff > -zeroEps_ &&
+ ldiff < zeroEps_;
+ asservTracker_.updateCounter (ldiff, rdiff, zero);
+ log_ (Log::verydebug) << "tracker update " << ldiff << ' ' << rdiff <<
+ std::endl;
// Retiens les anciennes valeurs.
countLeft_ = l;
countRight_ = r;
diff --git a/2004/i/nono/src/motor/asserv.h b/2004/i/nono/src/motor/asserv.h
index 60b9893..c391e41 100644
--- a/2004/i/nono/src/motor/asserv.h
+++ b/2004/i/nono/src/motor/asserv.h
@@ -36,7 +36,9 @@ class AsservTracker
{
public:
/// Appelée lors d'une mise à jour des compteurs.
- virtual void updateCounter (int l, int r) { }
+ /// l, r : mise à jour moteur gauche et droit (unit).
+ /// zero : vrai si le mouvement est à considérer comme nul.
+ virtual void updateCounter (int l, int r, bool zero) { }
};
/// Classe de dialogue avec la carte d'asservissement.
@@ -52,6 +54,7 @@ class Asserv
bool counter_;
bool posAsserv_;
bool asserv_;
+ int zeroEps_;
bool noSetParam_;
// File d'emmission.
std::queue<std::string> sendQueue_;
@@ -84,10 +87,9 @@ class Asserv
void waitOk (void);
/// Lit et traite les messages de la cartes.
void read (void);
- /// @{
+ //@{
/// Change les paramètres de la carte.
void setAccel (int accel);
- int getAccel (void) { return accel_; }
void setKp (int kp);
void setKi (int ki);
void setKd (int kd);
@@ -95,7 +97,11 @@ class Asserv
void setCounter (bool fl = true);
void setPosAsserv (bool fl = true);
void setAsserv (bool fl = true);
- /// @}
+ //@}
+ /// Get factor to deduce speed scale from scale.
+ double getSpeedFactor (void) const;
+ /// Get factor to deduce accel scale from scale.
+ double getAccelFactor (void) const;
protected:
/// @{
/// Envoie un message.
diff --git a/2004/i/nono/src/motor/goto_circle.h b/2004/i/nono/src/motor/goto_circle.h
index 50b576b..511d11d 100644
--- a/2004/i/nono/src/motor/goto_circle.h
+++ b/2004/i/nono/src/motor/goto_circle.h
@@ -26,7 +26,7 @@
// }}}
#include "goto.h"
-/// Classe abstraite d'un fournisseur de point pour MovementGoto.
+/// Goto effectuant un cercle.
class GotoCircle : public Goto
{
double r_, a_;
diff --git a/2004/i/nono/src/motor/goto_hermite.cc b/2004/i/nono/src/motor/goto_hermite.cc
new file mode 100644
index 0000000..a1fc6c5
--- /dev/null
+++ b/2004/i/nono/src/motor/goto_hermite.cc
@@ -0,0 +1,140 @@
+// goto_hermite.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 "goto_hermite.h"
+
+#include <iostream>
+
+/// Constructeur.
+/// fa : angle final.
+GotoHermite::GotoHermite (double fa)
+ : step_ (0.0), ia_ (0.0), fa_ (fa)
+{
+ points_.push_back (Point ());
+}
+
+/// Ajoute un point au chemin.
+void
+GotoHermite::add (double x, double y)
+{
+ points_.push_back (Point (x, y));
+}
+
+/// Initialise le Goto, appelé au début de la trajectoire.
+void
+GotoHermite::init (const Tracker &t)
+{
+ double x, y, a;
+ t.getPos (x, y, a);
+ points_[0] = Point (x, y);
+ ia_ = a;
+}
+
+/// Fournit la distance au point final (mm), le point le plus loin à moins
+/// de distmax (mm) et renvois false si le Goto est fini.
+/// eps : distance (mm) en dessous de laquelle on considère que la destination
+/// est atteinte.
+/// distmax : distance (mm) maximale du point.
+/// dist : distance (mm) au point final.
+/// (x, y) : point intermédiaire (mm).
+bool
+GotoHermite::get (const Tracker &t, double distmax, double eps, double &dist,
+ double &x, double &y)
+{
+ return false;
+}
+
+/// Test le GotoHermite en affichant la liste des points générés.
+void
+GotoHermite::test (std::ostream &os) const
+{
+ Point p;
+ for (double s = 0.0; ; s += stepSize_)
+ {
+ if (!computePoint (s, p))
+ break;
+ std::cout << p << std::endl;
+ }
+}
+
+/// Calcule le point au pas step, renvois faux si fini.
+bool
+GotoHermite::computePoint (double step, Point &p) const
+{
+ // Vérifications d'overflow.
+ if (step < 0.0)
+ step = 0.0;
+ // Détermine la section correspondante.
+ int i = static_cast<int> (step);
+ if (i >= static_cast<int> (points_.size ()) - 1)
+ return false;
+ step -= i;
+ const Point &p1 = points_[i];
+ const Point &p2 = points_[i + 1];
+ // Calcule les fonction d'hermite.
+ double step2 = step * step;
+ double step3 = step2 * step;
+ double h2 = -2 * step3 + 3 * step2;
+ double h1 = -h2 + 1;
+ double h3 = step3 - 2 * step2 + step;
+ double h4 = step3 - step2;
+ // Calcule les tangeantes.
+ Point t1;
+ if (i > 0)
+ {
+ // Méthode de Catmull-Rom.
+ t1 = (p2 - points_[i - 1]) * tightness_;
+ }
+ else
+ {
+ // Projette le vecteur vers le point suivant sur le vecteur angle.
+ t1 = p1;
+ double cia = cos (ia_);
+ double sia = sin (ia_);
+ t1.x += cia * (p2.x - p1.x) + sia * (p2.y - p1.y);
+ t1.x += -sia * (p2.x - p1.x) + cia * (p2.y - p1.y);
+ }
+ Point t2;
+ if (i < static_cast<int> (points_.size ()) - 2)
+ {
+ // Méthode de Catmull-Rom.
+ t2 = (points_[i + 2] - p1) * tightness_;
+ }
+ else
+ {
+ // Projette le vecteur depuis le point précédent sur le vecteur angle.
+ t2 = p2;
+ double cia = cos (fa_);
+ double sia = sin (fa_);
+ t2.x += cia * (p2.x - p1.x) + sia * (p2.y - p1.y);
+ t2.x += -sia * (p2.x - p1.x) + cia * (p2.y - p1.y);
+ }
+ // Aplique Hermite.
+ p = p1 * h1;
+ p+= t1 * h3;
+ p+= t2 * h4;
+ p+= p2 * h2;
+ return true;
+}
+
diff --git a/2004/i/nono/src/motor/goto_hermite.h b/2004/i/nono/src/motor/goto_hermite.h
new file mode 100644
index 0000000..bc6fb7e
--- /dev/null
+++ b/2004/i/nono/src/motor/goto_hermite.h
@@ -0,0 +1,67 @@
+#ifndef goto_hermite_h
+#define goto_hermite_h
+// goto_hermite.h
+// 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 "goto.h"
+#include "utils/point.h"
+
+#include <vector>
+
+namespace std { class ostream; }
+
+/// Goto suivant une courbe d'hermite.
+class GotoHermite : public Goto
+{
+ typedef std::vector<Point> Points;
+ Points points_;
+ double step_;
+ static const double stepSize_ = 0.01;
+ static const double tightness_ = 0.5;
+ double ia_, fa_;
+ public:
+ /// Constructeur.
+ /// fa : angle final.
+ GotoHermite (double fa);
+ /// Ajoute un point au chemin.
+ void add (double x, double y);
+ /// Initialise le Goto, appelé au début de la trajectoire.
+ void init (const Tracker &t);
+ /// Fournit la distance au point final (mm), le point le plus loin à moins
+ /// de distmax (mm) et renvois false si le Goto est fini.
+ /// eps : distance (mm) en dessous de laquelle on considère que la
+ /// destination est atteinte.
+ /// distmax : distance (mm) maximale du point.
+ /// dist : distance (mm) au point final.
+ /// (x, y) : point intermédiaire (mm).
+ bool get (const Tracker &t, double distmax, double eps,
+ double &dist, double &x, double &y);
+ /// Test le GotoHermite en affichant la liste des points générés.
+ void test (std::ostream &os) const;
+ protected:
+ /// Calcule le point au pas step, renvois faux si fini.
+ bool computePoint (double step, Point &p) const;
+};
+
+#endif // goto_hermite_h
diff --git a/2004/i/nono/src/motor/goto_simple.h b/2004/i/nono/src/motor/goto_simple.h
index d773720..f8bfa1a 100644
--- a/2004/i/nono/src/motor/goto_simple.h
+++ b/2004/i/nono/src/motor/goto_simple.h
@@ -26,7 +26,7 @@
// }}}
#include "goto.h"
-/// Classe abstraite d'un fournisseur de point pour MovementGoto.
+/// Goto permettant d'atteindre un point.
class GotoSimple : public Goto
{
double dx_, dy_;
diff --git a/2004/i/nono/src/motor/motor.cc b/2004/i/nono/src/motor/motor.cc
index 45c9955..ddd00ce 100644
--- a/2004/i/nono/src/motor/motor.cc
+++ b/2004/i/nono/src/motor/motor.cc
@@ -33,7 +33,7 @@ Motor *Motor::instance_ = 0;
/// Constructeur.
Motor::Motor (void)
: asserv_ (*this), log_ ("motor"),
- s_ (1.0), vs_ (1.0), as_ (1.0),
+ s_ (1.0), vs_ (1.0), a_ (1.0),
brakeDistance_ (0.0),
movement_ (0), maxSpeed_ (0x40), minSpeed_ (0x04)
{
@@ -45,14 +45,18 @@ Motor::Motor (void)
{
if (!(
rc.get ("scale", s_) ||
- rc.get ("speedscale", vs_) ||
- rc.get ("accelscale", as_) ||
- rc.get ("brakedistance", brakeDistance_) ||
rc.get ("maxspeed", maxSpeed_) ||
rc.get ("minspeed", minSpeed_)
))
rc.noId ();
}
+ // Initialise l'échelle.
+ vs_ = s_ * asserv_.getSpeedFactor ();
+ a_ = (s_ > 0 ? s_ : -s_) * asserv_.getAccelFactor ();
+ brakeDistance_ = 0.5 * (maxSpeed_ * maxSpeed_ * vs_ * vs_) / a_;
+ log_ (Log::debug) << "scale " << vs_ << ' ' << a_ << ' ' << brakeDistance_
+ << std::endl;
+ // C'est partit !
asserv_.reset ();
}
@@ -63,13 +67,15 @@ Motor::~Motor (void)
delete movement_;
}
-/// Appelée lors d'une mise à jour des compteurs (unit).
+/// Appelée lors d'une mise à jour des compteurs.
+/// l, r : mise à jour moteur gauche et droit (unit).
+/// zero : vrai si le mouvement est à considérer comme nul.
void
-Motor::updateCounter (int l, int r)
+Motor::updateCounter (int l, int r, bool zero)
{
double dL = l * s_;
double dR = r * s_;
- tracker_.update (dL, dR);
+ tracker_.update (dL, dR, zero);
if (movement_)
if (!movement_->control ())
{
@@ -127,13 +133,25 @@ Motor::waitStopped (void)
void
Motor::speed (double l, double a, double dist)
{
- speed (computeSpeed (dist) * (l - a), computeSpeed (dist) * (l + a));
+ speedLimit (l - a, l + a, computeSpeed (dist));
}
/// Paramètre la vitesse des moteurs (-1..+1).
void
Motor::speed (double l, double r)
{
+ speedLimit (l, r, maxSpeed_);
+}
+
+/// Paramètre la vitesse des moteurs (-1..+1) avec une vitesse limite
+/// (unit).
+void
+Motor::speedLimit (double l, double r, double vLimit)
+{
+ // Il faut un minimum tout de même. La politique, c'est on avance.
+ if (vLimit < minSpeed_)
+ vLimit = minSpeed_;
+ // Convertie le (-1..+1) en (unit).
int vL = (int) ((vs_ > 0 ? l : -l) * maxSpeed_ * 0.5);
int vR = (int) ((vs_ > 0 ? r : -r) * maxSpeed_ * 0.5);
// Teste le dépassement de vitesse minimale.
@@ -146,41 +164,43 @@ Motor::speed (double l, double r)
vR = (int) ((vs_ > 0 ? r : -r) * maxSpeed_ * 0.5);
}
// Teste le dépassement de vitesse maximal.
- if (vL > maxSpeed_)
+ if (vL > vLimit)
{
- vL = maxSpeed_;
+ vL = static_cast<int> (vLimit);
vR = (int) (r / l * vL);
}
- else if (vL < -maxSpeed_)
+ else if (vL < -vLimit)
{
- vL = -maxSpeed_;
+ vL = static_cast<int> (-vLimit);
vR = (int) (r / l * vL);
}
- if (vR > maxSpeed_)
+ if (vR > vLimit)
{
- vR = maxSpeed_;
+ vR = static_cast<int> (vLimit);
vL = (int) (l / r * vR);
}
- else if (vR < -maxSpeed_)
+ else if (vR < -vLimit)
{
- vR = -maxSpeed_;
+ vR = static_cast<int> (-vLimit);
vL = (int) (l / r * vR);
}
// Envois la commande.
log_ (Log::debug) << "speed " << l << ' ' << r << ' ' << vL << ' '
- << vR << std::endl;;
+ << vR << ' ' << vLimit << std::endl;;
asserv_.speed (vL, vR);
}
-/// Calcule la vitesse en ligne droit afin de pouvoir freiner au bout
+/// Calcule la vitesse (unit) en ligne droit afin de pouvoir freiner au bout
/// d'une distance (mm).
double
Motor::computeSpeed (double dist) const
{
+ if (dist <= 0.0)
+ return 0.0;
if (dist > brakeDistance_)
- return 1.0;
+ return maxSpeed_;
else
- return dist / brakeDistance_;
+ return sqrt (2.0 * dist * a_) / (vs_ > 0 ? vs_ : -vs_);
}
/// Va vers un point, en formant un arc de cercle, renvois faux si atteind.
@@ -193,13 +213,13 @@ Motor::goToArc (double x, double y, double eps/*5.0*/)
{
log_ (Log::debug) << "goto arc " << l << ' ' << r;
if (l > 0.0)
- l = sqrt (2.0 * asserv_.getAccel () * as_ * l);
+ l = sqrt (2.0 * a_ * l);
else
- l = -sqrt (2.0 * asserv_.getAccel () * as_ * -l);
+ l = -sqrt (2.0 * a_ * -l);
if (r > 0.0)
- r = sqrt (2.0 * asserv_.getAccel () * as_ * r);
+ r = sqrt (2.0 * a_ * r);
else
- r = -sqrt (2.0 * asserv_.getAccel () * as_ * -r);
+ r = -sqrt (2.0 * a_ * -r);
log_ << ' ' << l << ' ' << r << std::endl;
speed (l, r);
return true;
diff --git a/2004/i/nono/src/motor/motor.h b/2004/i/nono/src/motor/motor.h
index a5e15db..e7312a9 100644
--- a/2004/i/nono/src/motor/motor.h
+++ b/2004/i/nono/src/motor/motor.h
@@ -42,7 +42,7 @@ class Motor : public AsservTracker
/// Pointeur vers l'instance unique.
static Motor *instance_;
/// Echelles (mm/unit, mm/s.unit, mm/s^2.unit).
- double s_, vs_, as_;
+ double s_, vs_, a_;
/// Distance de freinage à vitesse maximale.
double brakeDistance_;
/// Mouvement en cours.
@@ -56,8 +56,10 @@ class Motor : public AsservTracker
virtual ~Motor (void);
/// Récupère l'instance unique.
static Motor &getInstance (void) { return *instance_; }
- /// Appelée lors d'une mise à jour des compteurs (unit).
- void updateCounter (int l, int r);
+ /// Appelée lors d'une mise à jour des compteurs.
+ /// l, r : mise à jour moteur gauche et droit (unit).
+ /// zero : vrai si le mouvement est à considérer comme nul.
+ void updateCounter (int l, int r, bool zero);
/// Choisi le mouvement en cours.
void setMovement (Movement *m);
/// Active l'asservissement.
@@ -81,8 +83,11 @@ class Motor : public AsservTracker
void speed (double l, double a, double dist);
/// Paramètre la vitesse des moteurs (-1..+1).
void speed (double l, double r);
- /// Calcule la vitesse en ligne droit afin de pouvoir freiner au bout
- /// d'une distance (mm).
+ /// Paramètre la vitesse des moteurs (-1..+1) avec une vitesse limite
+ /// (unit).
+ void speedLimit (double l, double r, double vLimit);
+ /// Calcule la vitesse (unit) en ligne droit afin de pouvoir freiner au
+ /// bout d'une distance (mm).
double computeSpeed (double dist) const;
/// Va vers un point, en formant un arc de cercle, renvois faux si atteind.
/// \deprecated Utiliser goTo().
diff --git a/2004/i/nono/src/motor/movement_basic.cc b/2004/i/nono/src/motor/movement_basic.cc
index 656ca35..00a07ce 100644
--- a/2004/i/nono/src/motor/movement_basic.cc
+++ b/2004/i/nono/src/motor/movement_basic.cc
@@ -54,7 +54,6 @@ MovementBasic::control (void)
double dY = t_->getY () - sY_;
if (dX * dX + dY * dY >= lenght2_)
{
- a_->stop ();
return false;
}
m_->speed (speed_, speed_);
diff --git a/2004/i/nono/src/motor/movement_round_cruise.cc b/2004/i/nono/src/motor/movement_round_cruise.cc
new file mode 100644
index 0000000..d138370
--- /dev/null
+++ b/2004/i/nono/src/motor/movement_round_cruise.cc
@@ -0,0 +1,42 @@
+// movement_round_cruise.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_round_cruise.h"
+#include "motor.h"
+
+/// Constructeur.
+/// speed : vitesse (-1..+1).
+MovementRoundCruise::MovementRoundCruise (double speed)
+ : speed_ (speed)
+{
+}
+
+/// Controlle la vitesse, retourne faux si mouvement terminé.
+bool
+MovementRoundCruise::control (void)
+{
+ m_->speed (-speed_, speed_);
+ return true;
+}
+
diff --git a/2004/i/nono/src/motor/movement_round_cruise.h b/2004/i/nono/src/motor/movement_round_cruise.h
new file mode 100644
index 0000000..381934d
--- /dev/null
+++ b/2004/i/nono/src/motor/movement_round_cruise.h
@@ -0,0 +1,42 @@
+#ifndef movement_round_cruise_h
+#define movement_round_cruise_h
+// movement_round_cruise.h
+// 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.h"
+
+/// Mouvement de recherche, en rotation sur lui même.
+class MovementRoundCruise : public Movement
+{
+ /// Vitesse.
+ double speed_;
+ public:
+ /// Constructeur.
+ /// speed : vitesse (-1..+1).
+ MovementRoundCruise (double speed);
+ /// Controlle la vitesse, retourne faux si mouvement terminé.
+ bool control (void);
+};
+
+#endif // movement_round_cruise_h
diff --git a/2004/i/nono/src/motor/movement_types.h b/2004/i/nono/src/motor/movement_types.h
index 7fb1ec4..901deea 100644
--- a/2004/i/nono/src/motor/movement_types.h
+++ b/2004/i/nono/src/motor/movement_types.h
@@ -27,6 +27,8 @@
#include "movement_basic.h"
#include "movement_goto.h"
#include "movement_rotation.h"
+#include "movement_round_cruise.h"
+
#include "goto_simple.h"
#include "goto_circle.h"
diff --git a/2004/i/nono/src/motor/test_goto_hermite.cc b/2004/i/nono/src/motor/test_goto_hermite.cc
new file mode 100644
index 0000000..431347a
--- /dev/null
+++ b/2004/i/nono/src/motor/test_goto_hermite.cc
@@ -0,0 +1,37 @@
+// test_goto_hermite.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 "goto_hermite.h"
+
+#include <cstdlib>
+
+int
+main (int argc, char **argv)
+{
+ GotoHermite g (0);
+ for (int i = 1; i < argc - 1; i += 2)
+ g.add (atof (argv[i]), atof (argv[i + 1]));
+ g.test (std::cout);
+ return 0;
+}
diff --git a/2004/i/nono/src/motor/test_tracker.cc b/2004/i/nono/src/motor/test_tracker.cc
index 8dd448d..18d709f 100644
--- a/2004/i/nono/src/motor/test_tracker.cc
+++ b/2004/i/nono/src/motor/test_tracker.cc
@@ -67,7 +67,7 @@ main (int argc, char **argv)
{
double l = atof (argv[i]);
double r = atof (argv[i + 1]);
- t.update (l, r);
+ t.update (l, r, false);
std::cout << "update " << l << ' ' << r << " pos " << t <<
std::endl;
}
diff --git a/2004/i/nono/src/motor/tracker.cc b/2004/i/nono/src/motor/tracker.cc
index 270455a..381ed32 100644
--- a/2004/i/nono/src/motor/tracker.cc
+++ b/2004/i/nono/src/motor/tracker.cc
@@ -210,10 +210,14 @@ Tracker::computeArcs (double x, double y, double &l, double &r, double eps)
/// Met à jour la position.
void
-Tracker::update (double dL, double dR)
+Tracker::update (double dL, double dR, bool zero)
{
// Compte les zeros.
- if (dL == 0.0 && dR == 0.0)
+ if (zero)
+ {
+ zero_++;
+ }
+ else if (dL == 0.0 && dR == 0.0)
{
zero_++;
return;
@@ -226,8 +230,8 @@ Tracker::update (double dL, double dR)
// Avec a petit (c'est le cas, car f_ >> abs (dR - dL)), a ~= atan (a).
double dA = (dR - dL) / f_;
double dS = 0.5 * (dL + dR);
- log_ (Log::verydebug) << "update dL " << dL << " dR " << dR << "
- dA " << dA << " dS " << dS << std::endl;
+ log_ (Log::verydebug) << "update dL " << dL << " dR " << dR << " dA " <<
+ dA << " dS " << dS << std::endl;
// Si l'angle est petit, évite une division par presque 0.
if (dA < 0.0001 && dA > -0.0001)
{
diff --git a/2004/i/nono/src/motor/tracker.h b/2004/i/nono/src/motor/tracker.h
index 5e80a65..b58bc41 100644
--- a/2004/i/nono/src/motor/tracker.h
+++ b/2004/i/nono/src/motor/tracker.h
@@ -81,7 +81,7 @@ class Tracker
bool computeArcs (double x, double y, double &l, double &r, double eps)
const;
/// Met à jour la position.
- void update (double dL, double dR);
+ void update (double dL, double dR, bool zero);
};
/// Affiche la position.