summaryrefslogtreecommitdiff
path: root/2004/i/nono
diff options
context:
space:
mode:
authorschodet2004-05-09 18:03:51 +0000
committerschodet2004-05-09 18:03:51 +0000
commitfbf9fdbeb40baf2431da481d14edfa1d232dc184 (patch)
treeaf7ac178dfb692faf1f87bf14712df0a862fc4df /2004/i/nono
parent1bb779ae372de25e1af78dcad84170d5c31e5d43 (diff)
gpio
hermite limitation de la vitesse angulaire movement queue motor_cmd
Diffstat (limited to '2004/i/nono')
-rw-r--r--2004/i/nono/src/motor/Makefile.defs6
-rw-r--r--2004/i/nono/src/motor/asserv.cc49
-rw-r--r--2004/i/nono/src/motor/asserv.h11
-rw-r--r--2004/i/nono/src/motor/goto_hermite.cc82
-rw-r--r--2004/i/nono/src/motor/goto_hermite.h18
-rw-r--r--2004/i/nono/src/motor/motor.cc121
-rw-r--r--2004/i/nono/src/motor/motor.h28
-rw-r--r--2004/i/nono/src/motor/motor_cmd.cc147
-rw-r--r--2004/i/nono/src/motor/motor_cmd.h35
-rw-r--r--2004/i/nono/src/motor/movement_goto.cc8
-rw-r--r--2004/i/nono/src/motor/movement_types.h1
-rw-r--r--2004/i/nono/src/motor/test_goto_hermite.cc7
-rw-r--r--2004/i/nono/src/motor/test_motor.cc123
-rw-r--r--2004/i/nono/src/motor/tracker.cc15
-rw-r--r--2004/i/nono/src/motor/tracker.h7
15 files changed, 483 insertions, 175 deletions
diff --git a/2004/i/nono/src/motor/Makefile.defs b/2004/i/nono/src/motor/Makefile.defs
index 033783f..2453e0d 100644
--- a/2004/i/nono/src/motor/Makefile.defs
+++ b/2004/i/nono/src/motor/Makefile.defs
@@ -1,7 +1,7 @@
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
+test_motor_SOURCES = test_motor.cc motor_cmd.cc motor.a serial.a erreur.a \
+ config.a date.a utils.a logger.a
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 \
@@ -9,7 +9,7 @@ test_tracker_SOURCES = test_tracker.cc motor.a serial.a erreur.a config.a \
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
+ goto_simple.cc goto_circle.cc goto_hermite.cc
test_goto_hermite_SOURCES = goto_hermite.cc test_goto_hermite.cc tracker.cc \
config.a logger.a erreur.a
diff --git a/2004/i/nono/src/motor/asserv.cc b/2004/i/nono/src/motor/asserv.cc
index c806479..7be14f7 100644
--- a/2004/i/nono/src/motor/asserv.cc
+++ b/2004/i/nono/src/motor/asserv.cc
@@ -30,12 +30,14 @@
/// Constructeur.
Asserv::Asserv (AsservTracker &asservTracker)
: ttyspeed_ (0), accel_ (-1), kp_ (-1), ki_ (-1), kd_ (-1),
- statMotor_ (-1), counter_ (false), posAsserv_ (false), asserv_ (false),
+ statMotor_ (-1), counter_ (false), posAsserv_ (false), gpiDelay_ (0),
+ asserv_ (false),
zeroEps_ (0),
noSetParam_ (false),
inBufSize_ (64), inBufPos_ (0), inBuf_ (0),
firstCounter_ (true),
countLeft_ (0), countRight_ (0),
+ gpi_ (0),
asservTracker_ (asservTracker),
log_ ("asserv")
{
@@ -53,13 +55,14 @@ Asserv::Asserv (AsservTracker &asservTracker)
rc.get ("stats", statMotor_) ||
rc.get ("counter", counter_) ||
rc.get ("posasserv", posAsserv_) ||
+ rc.get ("gpi", gpiDelay_) ||
rc.get ("zeroeps", zeroEps_) ||
rc.get ("buffer", inBufSize_)
))
rc.noId ();
}
// Ouvre le port série.
- serial_.open (ttyname_.c_str (), ttyspeed_);
+ serial_.open (ttyname_, ttyspeed_);
// Alloue la mémoire du tampon d'entrée.
inBuf_ = new char[inBufSize_ + 2];
}
@@ -105,6 +108,8 @@ Asserv::setParam (void)
setStatMotor (statMotor_);
if (posAsserv_)
setPosAsserv (posAsserv_);
+ if (gpiDelay_)
+ setGpiDelay (gpiDelay_);
if (asserv_)
setAsserv (asserv_);
if (counter_)
@@ -217,6 +222,13 @@ Asserv::setPosAsserv (bool fl/*true*/)
}
void
+Asserv::setGpiDelay (int delay)
+{
+ send ('h', delay);
+ gpiDelay_ = delay;
+}
+
+void
Asserv::setAsserv (bool fl/*true*/)
{
send ('g', fl);
@@ -237,6 +249,13 @@ Asserv::getAccelFactor (void) const
return getSpeedFactor () / 0.004 / accel_;
}
+/// Envois la sortie GPO.
+void
+Asserv::sendGpo (unsigned int gpo)
+{
+ send ('k', static_cast<int> (gpo));
+}
+
/// Envoie un message.
void
Asserv::send (char com)
@@ -335,6 +354,10 @@ Asserv::handleMessage (void)
// Recois une nouvelle valeur pour les compteurs.
handleCounter ();
break;
+ case 'H':
+ // Recois un GPI.
+ handleGpi ();
+ break;
case 'T':
// TTL expiré, log un message.
log_ (Log::warning) << "ttl expired" << std::endl;
@@ -449,3 +472,25 @@ Asserv::handleCounter (void)
countLeft_ = l;
countRight_ = r;
}
+
+/// Traite les GPI.
+void
+Asserv::handleGpi (void)
+{
+ if (inBufPos_ != 2 + 2 + 1)
+ {
+ // Mauvaise transmission.
+ log_ (Log::warning) << "gpi error (n)" << std::endl;
+ return;
+ }
+ unsigned int gpi = hexUnsignedChar2int (inBuf_ + 2);
+ if (gpi == static_cast<unsigned int> (hexInvalid))
+ {
+ // Mauvaise transmission.
+ log_ (Log::warning) << "gpi error (?)" << std::endl;
+ return;
+ }
+ // Recopie la valeur.
+ gpi_ = gpi;
+}
+
diff --git a/2004/i/nono/src/motor/asserv.h b/2004/i/nono/src/motor/asserv.h
index c391e41..aa39324 100644
--- a/2004/i/nono/src/motor/asserv.h
+++ b/2004/i/nono/src/motor/asserv.h
@@ -53,6 +53,7 @@ class Asserv
int statMotor_;
bool counter_;
bool posAsserv_;
+ int gpiDelay_;
bool asserv_;
int zeroEps_;
bool noSetParam_;
@@ -64,6 +65,8 @@ class Asserv
// Anciènne valeur des compteur.
bool firstCounter_;
int countLeft_, countRight_;
+ // Valeur de l'entrée GPI.
+ unsigned int gpi_;
// Objet interessé par les stats.
AsservTracker &asservTracker_;
// Système de log.
@@ -96,12 +99,18 @@ class Asserv
void setStatMotor (int delay);
void setCounter (bool fl = true);
void setPosAsserv (bool fl = true);
+ void setGpiDelay (int delay);
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;
+ /// Récupère l'entrée GPI.
+ unsigned int getGpi (void) const
+ { return gpi_; }
+ /// Envois la sortie GPO.
+ void sendGpo (unsigned int gpo);
protected:
/// @{
/// Envoie un message.
@@ -120,6 +129,8 @@ class Asserv
void handleStatMotor (void);
/// Traite un message du compteur.
void handleCounter (void);
+ /// Traite les GPI.
+ void handleGpi (void);
};
#endif // asserv_h
diff --git a/2004/i/nono/src/motor/goto_hermite.cc b/2004/i/nono/src/motor/goto_hermite.cc
index a1fc6c5..9e47296 100644
--- a/2004/i/nono/src/motor/goto_hermite.cc
+++ b/2004/i/nono/src/motor/goto_hermite.cc
@@ -29,7 +29,18 @@
/// Constructeur.
/// fa : angle final.
GotoHermite::GotoHermite (double fa)
- : step_ (0.0), ia_ (0.0), fa_ (fa)
+ : step_ (0.0), lastDist_ (0.0), ia_ (0.0), fa_ (fa),
+ di_ (defaultDi_), df_ (defaultDf_)
+{
+ points_.push_back (Point ());
+}
+
+/// Constructeur.
+/// fa : angle final.
+/// di : longueur de la tangente initiale.
+/// df : longueur de la tangente finale.
+GotoHermite::GotoHermite (double fa, double di, double df)
+ : step_ (0.0), lastDist_ (0.0), ia_ (0.0), fa_ (fa), di_ (di), df_ (df)
{
points_.push_back (Point ());
}
@@ -38,7 +49,19 @@ GotoHermite::GotoHermite (double fa)
void
GotoHermite::add (double x, double y)
{
- points_.push_back (Point (x, y));
+ Point newPoint (x, y);
+ // Met à jour le tableau des distances.
+ double dist = newPoint.distTo (points_.back ());
+ for (Dists::iterator i = dists_.begin ();
+ i != dists_.end ();
+ ++i)
+ {
+ *i += dist;
+ }
+ dists_.push_back (0.0);
+ lastDist_ += dist;
+ // Ajoute le point.
+ points_.push_back (newPoint);
}
/// Initialise le Goto, appelé au début de la trajectoire.
@@ -47,7 +70,7 @@ GotoHermite::init (const Tracker &t)
{
double x, y, a;
t.getPos (x, y, a);
- points_[0] = Point (x, y);
+ lastPoint_ = points_[0] = Point (x, y);
ia_ = a;
}
@@ -62,7 +85,22 @@ bool
GotoHermite::get (const Tracker &t, double distmax, double eps, double &dist,
double &x, double &y)
{
- return false;
+ Point cur (t.getX (), t.getY ());
+ // Trouve le point intermédiaire.
+ double sqDistMax = distmax * distmax;
+ while (cur.sqDistTo (lastPoint_) < sqDistMax)
+ {
+ step_ += stepSize_;
+ if (!computePoint (step_, lastPoint_, lastDist_))
+ break;
+ }
+ // Trouve la distance au point final.
+ dist = lastDist_ + cur.distTo (lastPoint_);
+ if (eps > dist)
+ return false;
+ x = lastPoint_.x;
+ y = lastPoint_.y;
+ return true;
}
/// Test le GotoHermite en affichant la liste des points générés.
@@ -70,17 +108,19 @@ void
GotoHermite::test (std::ostream &os) const
{
Point p;
+ double dist;
for (double s = 0.0; ; s += stepSize_)
{
- if (!computePoint (s, p))
+ if (!computePoint (s, p, dist))
break;
- std::cout << p << std::endl;
+ std::cout << p << ' ' << dist << std::endl;
}
}
-/// Calcule le point au pas step, renvois faux si fini.
+/// Calcule le point au pas step et la distance restant à parcourir,
+/// renvois faux si fini.
bool
-GotoHermite::computePoint (double step, Point &p) const
+GotoHermite::computePoint (double step, Point &p, double &dist) const
{
// Vérifications d'overflow.
if (step < 0.0)
@@ -108,12 +148,9 @@ GotoHermite::computePoint (double step, Point &p) const
}
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);
+ t1.x = cos (ia_) * di_;
+ t1.y = sin (ia_) * di_;
}
Point t2;
if (i < static_cast<int> (points_.size ()) - 2)
@@ -123,18 +160,19 @@ GotoHermite::computePoint (double step, Point &p) const
}
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);
+ t2.x = cos (fa_) * df_;
+ t2.y = sin (fa_) * df_;
}
- // Aplique Hermite.
+ // Applique Hermite.
+ t1 *= h3;
+ t2 *= h4;
p = p1 * h1;
- p+= t1 * h3;
- p+= t2 * h4;
- p+= p2 * h2;
+ p += t1;
+ p += t2;
+ p += p2 * h2;
+ // Calcule la distance au point final.
+ dist = dists_[i] + p.distTo (points_[i + 1]);
return true;
}
diff --git a/2004/i/nono/src/motor/goto_hermite.h b/2004/i/nono/src/motor/goto_hermite.h
index bc6fb7e..8815e12 100644
--- a/2004/i/nono/src/motor/goto_hermite.h
+++ b/2004/i/nono/src/motor/goto_hermite.h
@@ -36,14 +36,25 @@ class GotoHermite : public Goto
{
typedef std::vector<Point> Points;
Points points_;
+ typedef std::vector<double> Dists;
+ Dists dists_;
double step_;
+ Point lastPoint_;
+ double lastDist_;
static const double stepSize_ = 0.01;
static const double tightness_ = 0.5;
- double ia_, fa_;
+ static const double defaultDi_ = 0.0;
+ static const double defaultDf_ = 600.0;
+ double ia_, fa_, di_, df_;
public:
/// Constructeur.
/// fa : angle final.
GotoHermite (double fa);
+ /// Constructeur.
+ /// fa : angle final.
+ /// di : longueur de la tangente initiale.
+ /// df : longueur de la tangente finale.
+ GotoHermite (double fa, double di, double df);
/// Ajoute un point au chemin.
void add (double x, double y);
/// Initialise le Goto, appelé au début de la trajectoire.
@@ -60,8 +71,9 @@ class GotoHermite : public Goto
/// 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;
+ /// Calcule le point au pas step et la distance restant à parcourir,
+ /// renvois faux si fini.
+ bool computePoint (double step, Point &p, double &dist) const;
};
#endif // goto_hermite_h
diff --git a/2004/i/nono/src/motor/motor.cc b/2004/i/nono/src/motor/motor.cc
index ddd00ce..902e46f 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), a_ (1.0),
+ s_ (1.0), vs_ (1.0), a_ (1.0), fact_ (0.5),
brakeDistance_ (0.0),
movement_ (0), maxSpeed_ (0x40), minSpeed_ (0x04)
{
@@ -45,6 +45,7 @@ Motor::Motor (void)
{
if (!(
rc.get ("scale", s_) ||
+ rc.get ("fact", fact_) ||
rc.get ("maxspeed", maxSpeed_) ||
rc.get ("minspeed", minSpeed_)
))
@@ -63,8 +64,8 @@ Motor::Motor (void)
/// Destructeur.
Motor::~Motor (void)
{
+ clearMovement ();
instance_ = 0;
- delete movement_;
}
/// Appelée lors d'une mise à jour des compteurs.
@@ -76,22 +77,43 @@ Motor::updateCounter (int l, int r, bool zero)
double dL = l * s_;
double dR = r * s_;
tracker_.update (dL, dR, zero);
+ if (!movement_)
+ {
+ if (!nextMovement ())
+ return;
+ }
if (movement_)
- if (!movement_->control ())
+ {
+ while (!movement_->control ())
{
- delete movement_;
- movement_ = 0;
- asserv_.stop ();
+ if (!nextMovement ())
+ {
+ // C'est la fin.
+ asserv_.stop ();
+ return;
+ }
}
+ }
+
+
+}
+
+/// Ajoute un mouvement à la file d'attente.
+void
+Motor::addMovement (Movement *m)
+{
+ movementQueue_.push_back (m);
}
-/// Choisi le mouvement en cours.
+/// Vide la file d'attente.
void
-Motor::setMovement (Movement *m)
+Motor::clearMovement (void)
{
delete movement_;
- movement_ = m;
- m->init (tracker_, asserv_, *this);
+ MovementQueue::iterator first = movementQueue_.begin ();
+ MovementQueue::iterator last = movementQueue_.end ();
+ for ( ; first != last; ++first)
+ delete *first;
}
/// Teste si les mouvements sont terminés.
@@ -99,7 +121,7 @@ bool
Motor::end (void)
{
ok ();
- return !movement_;
+ return !movement_ && movementQueue_.empty ();
}
/// Attend la fin de tout les mouvements.
@@ -129,64 +151,83 @@ Motor::waitStopped (void)
}
/// Paramètre la vitesse linéaire (-1..+1) et angulaire (-1..+1). Limite
-/// la vitesse pour pouvoir freiner au bout de dist (mm).
+/// la vitesse pour pouvoir freiner au bout de dist (mm) et au bout de
+/// l'angle (rad).
void
-Motor::speed (double l, double a, double dist)
+Motor::speed (double l, double a, double dist, double angle)
{
- speedLimit (l - a, l + a, computeSpeed (dist));
+ double liml, lima;
+ liml = computeSpeed (dist);
+ lima = computeSpeed (tracker_.getAngleDistance (angle));
+ speedLimit (l - a, l + a, liml, lima);
}
/// Paramètre la vitesse des moteurs (-1..+1).
void
Motor::speed (double l, double r)
{
- speedLimit (l, r, maxSpeed_);
+ speedLimit (l, r, maxSpeed_, 2 * maxSpeed_);
}
/// Paramètre la vitesse des moteurs (-1..+1) avec une vitesse limite
-/// (unit).
+/// linéaire en angulaire (unit).
void
-Motor::speedLimit (double l, double r, double vLimit)
+Motor::speedLimit (double l, double r, double vLimitL, double vLimitA)
{
// Il faut un minimum tout de même. La politique, c'est on avance.
- if (vLimit < minSpeed_)
- vLimit = minSpeed_;
+ if (vLimitL < minSpeed_)
+ vLimitL = minSpeed_;
+ if (vLimitA < minSpeed_)
+ vLimitA = 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);
+ int vL = (int) ((vs_ > 0 ? l : -l) * maxSpeed_ * fact_);
+ int vR = (int) ((vs_ > 0 ? r : -r) * maxSpeed_ * fact_);
// 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) ((vs_ > 0 ? l : -l) * maxSpeed_ * 0.5);
- vR = (int) ((vs_ > 0 ? r : -r) * maxSpeed_ * 0.5);
+ vL = (int) ((vs_ > 0 ? l : -l) * maxSpeed_ * fact_);
+ vR = (int) ((vs_ > 0 ? r : -r) * maxSpeed_ * fact_);
}
- // Teste le dépassement de vitesse maximal.
- if (vL > vLimit)
+ // Teste le dépassement de vitesse maximale linéaire.
+ if (vL > vLimitL)
{
- vL = static_cast<int> (vLimit);
+ vL = static_cast<int> (vLimitL);
vR = (int) (r / l * vL);
}
- else if (vL < -vLimit)
+ else if (vL < -vLimitL)
{
- vL = static_cast<int> (-vLimit);
+ vL = static_cast<int> (-vLimitL);
vR = (int) (r / l * vL);
}
- if (vR > vLimit)
+ if (vR > vLimitL)
{
- vR = static_cast<int> (vLimit);
+ vR = static_cast<int> (vLimitL);
vL = (int) (l / r * vR);
}
- else if (vR < -vLimit)
+ else if (vR < -vLimitL)
{
- vR = static_cast<int> (-vLimit);
+ vR = static_cast<int> (-vLimitL);
vL = (int) (l / r * vR);
}
+ // Teste le dépassement de vitesse maximale angulaire.
+ if (vL - vR > vLimitA)
+ {
+ double fact = vLimitA / (vL - vR);
+ vR = static_cast<int> (vR * fact);
+ vL = static_cast<int> (vL * fact);
+ }
+ else if (vR - vL > vLimitA)
+ {
+ double fact = vLimitA / (vR - vL);
+ vR = static_cast<int> (vR * fact);
+ vL = static_cast<int> (vL * fact);
+ }
// Envois la commande.
log_ (Log::debug) << "speed " << l << ' ' << r << ' ' << vL << ' '
- << vR << ' ' << vLimit << std::endl;;
+ << vR << ' ' << vLimitL << ' ' << vLimitA << std::endl;;
asserv_.speed (vL, vR);
}
@@ -231,3 +272,17 @@ Motor::goToArc (double x, double y, double eps/*5.0*/)
}
}
+/// Passe au mouvement suivant.
+bool
+Motor::nextMovement (void)
+{
+ delete movement_;
+ movement_ = 0;
+ if (movementQueue_.empty ())
+ return false;
+ movement_ = movementQueue_.front ();
+ movementQueue_.pop_front ();
+ movement_->init (tracker_, asserv_, *this);
+ return true;
+}
+
diff --git a/2004/i/nono/src/motor/motor.h b/2004/i/nono/src/motor/motor.h
index e7312a9..b33ede3 100644
--- a/2004/i/nono/src/motor/motor.h
+++ b/2004/i/nono/src/motor/motor.h
@@ -28,6 +28,8 @@
#include "tracker.h"
#include "movement.h"
+#include <list>
+
/// Gestion des moteurs principaux du robot. Cette classe gére les
/// déplacements simples (enfin pas si simple que ça) comme aller vers un
/// point, atteindre un angle...
@@ -43,9 +45,13 @@ class Motor : public AsservTracker
static Motor *instance_;
/// Echelles (mm/unit, mm/s.unit, mm/s^2.unit).
double s_, vs_, a_;
+ /// Echelles entre l'unitée et la vitesse max.
+ double fact_;
/// Distance de freinage à vitesse maximale.
double brakeDistance_;
- /// Mouvement en cours.
+ /// File de mouvements.
+ typedef std::list<Movement *> MovementQueue;
+ MovementQueue movementQueue_;
Movement *movement_;
/// Vitesses maximale et minimale (unit).
int maxSpeed_, minSpeed_;
@@ -60,8 +66,10 @@ class Motor : public AsservTracker
/// 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);
+ /// Ajoute un mouvement à la file d'attente.
+ void addMovement (Movement *m);
+ /// Vide la file d'attente.
+ void clearMovement (void);
/// Active l'asservissement.
void go (bool fl = true) { asserv_.setAsserv (fl); }
/// Teste si l'émission est terminée.
@@ -78,20 +86,26 @@ class Motor : public AsservTracker
void waitStopped (void);
/// Récupère l'object tracker.
const Tracker &getTracker (void) const { return tracker_; }
+ /// Récupère l'object asserv.
+ Asserv &getAsserv (void) { return asserv_; }
/// Paramètre la vitesse linéaire (-1..+1) et angulaire (-1..+1). Limite
- /// la vitesse pour pouvoir freiner au bout de dist (mm).
- void speed (double l, double a, double dist);
+ /// la vitesse pour pouvoir freiner au bout de dist (mm) et au bout de
+ /// l'angle (rad).
+ void speed (double l, double a, double dist, double angle);
/// Paramètre la vitesse des moteurs (-1..+1).
void speed (double l, double r);
/// Paramètre la vitesse des moteurs (-1..+1) avec une vitesse limite
- /// (unit).
- void speedLimit (double l, double r, double vLimit);
+ /// linéaire en angulaire (unit).
+ void speedLimit (double l, double r, double vLimitL, double vLimitA);
/// 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().
bool goToArc (double x, double y, double eps = 5.0);
+ protected:
+ /// Passe au mouvement suivant.
+ bool nextMovement (void);
};
#endif // motor_h
diff --git a/2004/i/nono/src/motor/motor_cmd.cc b/2004/i/nono/src/motor/motor_cmd.cc
new file mode 100644
index 0000000..578ed3d
--- /dev/null
+++ b/2004/i/nono/src/motor/motor_cmd.cc
@@ -0,0 +1,147 @@
+// motor_cmd.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 "motor_cmd.h"
+#include "motor.h"
+#include "movement_types.h"
+
+#include <iostream>
+#include <cmath>
+
+const char motorHelp[] =
+ " g go (active l'asservissement)\n"
+ " G ungo (désactive l'asservissement)\n"
+ " w wait stop (attend l'arret complet)\n"
+ " b <speed> <len>\n"
+ " mouvement basique\n"
+ " t <x> <y> va en (x, y)\n"
+ " r <a> se dirige vers l'angle a en degrés\n"
+ " c <r> <a> Suis un arc de rayon r et d'angle a en degrés\n"
+ " h <fa> [p <x> <y>]...\n"
+ " trace une courbe d'hermite\n";
+
+bool
+motorCmd (int argc, char **argv, int &i, Motor &m)
+{
+ switch (argv[i][0])
+ {
+ case 'g':
+ std::cout << "test: go" << std::endl;
+ i++;
+ m.go ();
+ break;
+ case 'G':
+ std::cout << "test: ungo" << std::endl;
+ i++;
+ m.go (false);
+ break;
+ case 'w':
+ std::cout << "test: wait stop" << std::endl;
+ i++;
+ m.waitStopped ();
+ break;
+ case 'b':
+ {
+ i++;
+ double speed;
+ double len;
+ if (i >= argc) break;
+ speed = atof (argv[i++]);
+ if (i >= argc) break;
+ len = atof (argv[i++]);
+ std::cout << "test: basic " << speed << ' ' << len << std::endl;
+ Movement *mov = new MovementBasic (speed, len);
+ m.addMovement (mov);
+ }
+ break;
+ case 't':
+ {
+ i++;
+ double dX, dY;
+ if (i >= argc) break;
+ dX = atof (argv[i++]);
+ if (i >= argc) break;
+ dY = atof (argv[i++]);
+ std::cout << "test: goto " << dX << ' ' << dY << std::endl;
+ Goto *g = new GotoSimple (dX, dY);
+ Movement *mov = new MovementGoto (g);
+ m.addMovement (mov);
+ }
+ break;
+ case 'r':
+ {
+ i++;
+ double dA;
+ if (i >= argc) break;
+ dA = atof (argv[i++]);
+ std::cout << "test: rotation " << dA << std::endl;
+ Movement *mov = new MovementRotation (dA * 2 * M_PI /
+ 360);
+ m.addMovement (mov);
+ }
+ break;
+ case 'c':
+ {
+ i++;
+ double r, a;
+ if (i >= argc) break;
+ r = atof (argv[i++]);
+ if (i >= argc) break;
+ a = atof (argv[i++]);
+ std::cout << "test: circle " << r << ' ' << a << std::endl;
+ Goto *g = new GotoCircle (r, a * 2 * M_PI / 360);
+ Movement *mov = new MovementGoto (g);
+ m.addMovement (mov);
+ }
+ break;
+ case 'h':
+ {
+ i++;
+ double fa, x, y;
+ if (i >= argc) break;
+ fa = atof (argv[i++]);
+ std::cout << "test: hermite " << fa << ' ' << x << ' ' << y <<
+ std::endl;
+ GotoHermite *g = new GotoHermite (fa * 2 * M_PI / 360);
+ while (argv[i][0] == 'p')
+ {
+ i++;
+ if (i >= argc) break;
+ x = atof (argv[i++]);
+ if (i >= argc) break;
+ y = atof (argv[i++]);
+ std::cout << "test: hermite point " << x << ' ' << y <<
+ std::endl;
+ g->add (x, y);
+ }
+ Movement *mov = new MovementGoto (g);
+ m.addMovement (mov);
+ }
+ break;
+ default:
+ return false;
+ }
+ return true;
+}
+
diff --git a/2004/i/nono/src/motor/motor_cmd.h b/2004/i/nono/src/motor/motor_cmd.h
new file mode 100644
index 0000000..a89e94f
--- /dev/null
+++ b/2004/i/nono/src/motor/motor_cmd.h
@@ -0,0 +1,35 @@
+#ifndef motor_cmd_h
+#define motor_cmd_h
+// motor_cmd.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.
+//
+// }}}
+
+class Motor;
+
+extern const char motorHelp[];
+
+bool
+motorCmd (int argc, char **argv, int &i, Motor &m);
+
+#endif // motor_cmd_h
diff --git a/2004/i/nono/src/motor/movement_goto.cc b/2004/i/nono/src/motor/movement_goto.cc
index cc720a9..efaf959 100644
--- a/2004/i/nono/src/motor/movement_goto.cc
+++ b/2004/i/nono/src/motor/movement_goto.cc
@@ -88,15 +88,15 @@ MovementGoto::control (void)
std::cout << "movement goto: consign " << dist << ' ' << dx << ' ' << dy <<
std::endl;
// Calcule l'erreur.
- double el, ea;
- t_->computeError (dx, dy, el, ea);
+ double el, ea, angle;
+ t_->computeError (dx, dy, el, ea, angle);
// 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;
+std::cout << "movement goto: error " << el << ' ' << ea << ' ' << angle << std::endl;
// Calcule les intégrales saturées.
il_ += el;
if (il_ > param_.is_) il_ = param_.is_;
@@ -110,7 +110,7 @@ std::cout << "movement goto: error " << el << ' ' << ea << std::endl;
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);
+ m_->speed (l, a, dist, angle);
// Retiens l'erreur pour la dérivée.
lel_ = el;
lea_ = ea;
diff --git a/2004/i/nono/src/motor/movement_types.h b/2004/i/nono/src/motor/movement_types.h
index 901deea..ca80715 100644
--- a/2004/i/nono/src/motor/movement_types.h
+++ b/2004/i/nono/src/motor/movement_types.h
@@ -31,5 +31,6 @@
#include "goto_simple.h"
#include "goto_circle.h"
+#include "goto_hermite.h"
#endif // movement_types_h
diff --git a/2004/i/nono/src/motor/test_goto_hermite.cc b/2004/i/nono/src/motor/test_goto_hermite.cc
index 431347a..bfa8b1e 100644
--- a/2004/i/nono/src/motor/test_goto_hermite.cc
+++ b/2004/i/nono/src/motor/test_goto_hermite.cc
@@ -25,10 +25,17 @@
#include "goto_hermite.h"
#include <cstdlib>
+#include <iostream>
int
main (int argc, char **argv)
{
+ if (argc < 3)
+ {
+ std::cerr <<
+ "test_goto_hermite - teste la généation de courbes d'hermite.\n"
+ " <x> <y> destination" << std::endl;
+ }
GotoHermite g (0);
for (int i = 1; i < argc - 1; i += 2)
g.add (atof (argv[i]), atof (argv[i + 1]));
diff --git a/2004/i/nono/src/motor/test_motor.cc b/2004/i/nono/src/motor/test_motor.cc
index 42130bf..b393bb7 100644
--- a/2004/i/nono/src/motor/test_motor.cc
+++ b/2004/i/nono/src/motor/test_motor.cc
@@ -23,14 +23,12 @@
//
// }}}
#include "motor.h"
-#include "erreur/erreur.h"
#include "date/date.h"
-#include "movement_types.h"
+#include "motor_cmd.h"
#include <iostream>
#include <unistd.h>
-#include <stdlib.h>
-#include <math.h>
+#include <cstdlib>
using namespace std;
@@ -39,15 +37,8 @@ syntax (void)
{
std::cout <<
"test_motor - teste la gestion du déplacement.\n"
- " g go (active l'asservissement)\n"
- " G ungo (désactive l'asservissement)\n"
- " w wait stop (attend l'arret complet)\n"
- " b <speed> <len>\n"
- " mouvement basique\n"
- " t <x> <y> va en (x, y)\n"
- " r <a> se dirige vers l'angle a en degrés\n"
- " c <r> <a> Suis un arc de rayon r et d'angle a en degrés\n"
- " <timeout> attend un certain temps (qui augmente avec timeout)\n"
+ << motorHelp <<
+ " <timeout> attend (ms)\n"
" ? cet ecran d'aide" << std::endl;
}
@@ -60,95 +51,31 @@ main (int argc, char **argv)
int i = 1;
while (i < argc)
{
- switch (argv[i][0])
+ if (!motorCmd (argc, argv, i, m))
{
- case 'g':
- cout << "test: go" << endl;
- i++;
- m.go ();
- break;
- case 'G':
- cout << "test: ungo" << endl;
- i++;
- m.go (false);
- break;
- case 'w':
- cout << "test: wait stop" << endl;
- i++;
- m.waitStopped ();
- break;
- case 'b':
- {
- i++;
- double speed;
- double len;
- if (i >= argc) break;
- speed = atof (argv[i++]);
- if (i >= argc) break;
- len = atof (argv[i++]);
- cout << "test: basic " << speed << ' ' << len << endl;
- Movement *mov = new MovementBasic (speed, len);
- m.setMovement (mov);
- }
- break;
- case 't':
- {
- i++;
- double dX, dY;
- if (i >= argc) break;
- dX = atof (argv[i++]);
- if (i >= argc) break;
- dY = atof (argv[i++]);
- cout << "test: goto " << dX << ' ' << dY << endl;
- Goto *g = new GotoSimple (dX, dY);
- Movement *mov = new MovementGoto (g);
- m.setMovement (mov);
- }
- break;
- case 'r':
- {
- i++;
- double dA;
- if (i >= argc) break;
- dA = atof (argv[i++]);
- cout << "test: rotation " << dA << endl;
- Movement *mov = new MovementRotation (dA * 2 * M_PI /
- 360);
- m.setMovement (mov);
- }
- break;
- case 'c':
- {
- i++;
- double r, a;
- if (i >= argc) break;
- r = atof (argv[i++]);
- if (i >= argc) break;
- a = atof (argv[i++]);
- cout << "test: circle " << r << ' ' << a << endl;
- Goto *g = new GotoCircle (r, a * 2 * M_PI / 360);
- Movement *mov = new MovementGoto (g);
- m.setMovement (mov);
- }
- break;
- case '?':
- syntax ();
- return 0;
- default:
- int s;
- s = atoi (argv[i++]);
- if (s == 0)
+ switch (argv[i][0])
{
+ case '?':
syntax ();
- return 1;
- }
- cout << "test: sleep " << s << endl;
- for (int j = 0; j < s * 1000 / 50; ++j)
- {
- m.ok ();
- Date::wait (50);
+ return 0;
+ default:
+ int s;
+ s = atoi (argv[i++]);
+ if (s == 0)
+ {
+ syntax ();
+ return 1;
+ }
+ cout << "test: sleep " << s << endl;
+ int start;
+ start = Date::getInstance ().start ();
+ while (Date::getInstance ().start () < start + s)
+ {
+ m.ok ();
+ Date::wait (50);
+ }
+ break;
}
- break;
}
m.waitEnd ();
}
diff --git a/2004/i/nono/src/motor/tracker.cc b/2004/i/nono/src/motor/tracker.cc
index 381ed32..4bd0e7b 100644
--- a/2004/i/nono/src/motor/tracker.cc
+++ b/2004/i/nono/src/motor/tracker.cc
@@ -75,6 +75,13 @@ Tracker::getAngleDiff (double a) const
return angleNorm (angle_ - a);
}
+/// Récupère la distance à parcourir avec chaque roue pour parcourir un angle.
+double
+Tracker::getAngleDistance (double angleDiff) const
+{
+ return 0.5 * f_ * fabs (angleDiff);
+}
+
/// Récupère les coordonnées d'un point à moins de dist (mm), dans la
/// direction de (dx, dy).
void
@@ -101,10 +108,12 @@ Tracker::getPoint (double dx, double dy, double &x, double &y, double dist)
/// Calcule l'erreur linéaire et angulaire pour atteindre un point.
/// (x, y) : point à atteindre (mm).
/// el, ea : erreur linéaire et angulaire (-1..+1).
+/// a : angle.
void
-Tracker::computeError (double x, double y, double &el, double &ea) const
+Tracker::computeError (double x, double y, double &el, double &ea, double &a)
+ const
{
- // Calcule la diférence avec la cible voulue.
+ // Calcule la différence avec la cible voulue.
double eX = x - posX_;
double eY = y - posY_;
// Calcule la distance.
@@ -120,6 +129,8 @@ Tracker::computeError (double x, double y, double &el, double &ea) const
// Calcule l'erreur angulaire (projection sur la direction
// perpendiculaire).
ea = eX * -sa + eY * ca;
+ // Calcule l'angle.
+ a = ea > 0.0 ? acos (el) : -acos (el);
}
/// Calcule l'erreur angulaire pour atteindre un angle, retourne faux si
diff --git a/2004/i/nono/src/motor/tracker.h b/2004/i/nono/src/motor/tracker.h
index b58bc41..96cf6f0 100644
--- a/2004/i/nono/src/motor/tracker.h
+++ b/2004/i/nono/src/motor/tracker.h
@@ -57,6 +57,9 @@ class Tracker
double getAngle (void) const { return angle_; }
/// Get distance.
double getDistance (double x, double y) const;
+ /// Récupère la distance à parcourir avec chaque roue pour parcourir un
+ /// angle.
+ double getAngleDistance (double angleDiff) const;
/// Récupère les coordonnées d'un point à moins de dist (mm), dans la
/// direction de (dx, dy).
void getPoint (double dx, double dy, double &x, double &y, double dist)
@@ -68,7 +71,9 @@ class Tracker
/// Calcule l'erreur linéaire et angulaire pour atteindre un point.
/// (x, y) : point à atteindre (mm).
/// el, ea : erreur linéaire et angulaire (-1..+1).
- void computeError (double x, double y, double &el, double &ea) const;
+ /// a : angle.
+ void computeError (double x, double y, double &el, double &ea, double &a)
+ const;
/// Calcule l'erreur angulaire pour atteindre un angle, retourne faux si
/// atteind.
/// a : angle à atteindre (rad).