summaryrefslogtreecommitdiff
path: root/2004/i
diff options
context:
space:
mode:
authorschodet2004-04-16 16:46:46 +0000
committerschodet2004-04-16 16:46:46 +0000
commita3a3a4d6d183b22036a606cda8553192c98321cd (patch)
tree37d158408eabfbb8524d1214a985a44e376ee333 /2004/i
parente68b824329a537eddddd5b59b9bdf498698ee604 (diff)
Logger fonctionne.
MovementRotation aussi. Ajout de Serial::wait. Ajout de commentaires.
Diffstat (limited to '2004/i')
-rw-r--r--2004/i/nono/runtime/rc/logger3
-rw-r--r--2004/i/nono/runtime/rc/movement/goto4
-rw-r--r--2004/i/nono/src/logger/log.cc85
-rw-r--r--2004/i/nono/src/logger/log.h22
-rw-r--r--2004/i/nono/src/logger/logger.cc8
-rw-r--r--2004/i/nono/src/logger/logger.h4
-rw-r--r--2004/i/nono/src/motor/asserv.cc12
-rw-r--r--2004/i/nono/src/motor/asserv.h2
-rw-r--r--2004/i/nono/src/motor/motor.cc20
-rw-r--r--2004/i/nono/src/motor/motor.h4
-rw-r--r--2004/i/nono/src/motor/movement.h10
-rw-r--r--2004/i/nono/src/motor/movement_basic.cc10
-rw-r--r--2004/i/nono/src/motor/movement_basic.h17
-rw-r--r--2004/i/nono/src/motor/movement_goto.cc5
-rw-r--r--2004/i/nono/src/motor/movement_goto.h16
-rw-r--r--2004/i/nono/src/motor/movement_rotation.cc84
-rw-r--r--2004/i/nono/src/motor/movement_rotation.h67
-rw-r--r--2004/i/nono/src/motor/test_asserv.cc25
-rw-r--r--2004/i/nono/src/motor/test_motor.cc27
-rw-r--r--2004/i/nono/src/motor/tracker.cc84
-rw-r--r--2004/i/nono/src/motor/tracker.h49
-rw-r--r--2004/i/nono/src/serial/serial.h3
-rw-r--r--2004/i/nono/src/serial/serial_base.cc41
-rw-r--r--2004/i/nono/src/serial/serial_base.h3
-rw-r--r--2004/i/nono/src/utils/errno_exception.h25
25 files changed, 514 insertions, 116 deletions
diff --git a/2004/i/nono/runtime/rc/logger b/2004/i/nono/runtime/rc/logger
new file mode 100644
index 0000000..379b824
--- /dev/null
+++ b/2004/i/nono/runtime/rc/logger
@@ -0,0 +1,3 @@
+motor debug
+asserv info
+tracker info
diff --git a/2004/i/nono/runtime/rc/movement/goto b/2004/i/nono/runtime/rc/movement/goto
index fb72688..874538a 100644
--- a/2004/i/nono/runtime/rc/movement/goto
+++ b/2004/i/nono/runtime/rc/movement/goto
@@ -1,5 +1,5 @@
epsilon 100
distance 150
-kpl 2.0 kil 0.1 kdl 0.7
-kpa 1.5 kia 0.1 kda 0.7
+kpl 1.0 kil 0.1 kdl 0.7
+kpa 1.0 kia 0.1 kda 0.7
is 1000
diff --git a/2004/i/nono/src/logger/log.cc b/2004/i/nono/src/logger/log.cc
index 4dc4f59..89f4f91 100644
--- a/2004/i/nono/src/logger/log.cc
+++ b/2004/i/nono/src/logger/log.cc
@@ -53,43 +53,88 @@ Log::operator() (Level level)
return *this;
}
-static const char *levelTab[] =
-{
- "none",
- "fatal",
- "error",
- "warning",
- "info",
- "debug",
- "verydebug"
-};
-
/// Traduit le niveau de log.
void
Log::toString (Level level, std::string &s)
{
- s = levelTab[static_cast<int> (level)];
+ switch (level)
+ {
+ case none:
+ s = "none"; break;
+ case fatal:
+ s = "fatal"; break;
+ case error:
+ s = "error"; break;
+ case warning:
+ s = "warning"; break;
+ case info:
+ s = "info"; break;
+ case debug:
+ s = "debug"; break;
+ case verydebug:
+ s = "verydebug"; break;
+ default:
+ s = "logunknown"; break;
+ }
}
/// Traduit le niveau de log.
Log::Level
Log::toLevel (const std::string &level)
{
- for (int i = 0; i < static_cast<int> (nbLevel); ++i)
+ switch (level[0])
+ {
+ case 'n':
+ return none;
+ case 'f':
+ return fatal;
+ case 'e':
+ return error;
+ case 'w':
+ return warning;
+ case 'i':
+ return info;
+ case 'd':
+ return debug;
+ case 'v':
+ return verydebug;
+ default:
+ return levelReserved;
+ }
+}
+
+/// Traduit le masque de niveau de log.
+Log::Level
+Log::toLevelMask (const std::string &level)
+{
+ switch (level[0])
{
- if (level == levelTab[i])
- return static_cast<Level> (i);
+ case 'n':
+ return static_cast<Level> (none);
+ case 'f':
+ return static_cast<Level> (fatal);
+ case 'e':
+ return static_cast<Level> (error | fatal);
+ case 'w':
+ return static_cast<Level> (warning | error | fatal);
+ case 'i':
+ return static_cast<Level> (info | warning | error | fatal);
+ case 'd':
+ return static_cast<Level> (debug | info | warning | error | fatal);
+ case 'v':
+ return static_cast<Level> (verydebug | debug | info | warning | error | fatal);
+ default:
+ return toLevel (level);
}
- return nbLevel;
}
/// Appellé lorsque le tampon streambuf déborde.
int
Log::overflow (int c)
{
-/* if (c == '\n')
+ if (c == '\n')
{
- if (curLevel_ <= maxLevel_)
+ if (curLevel_ & levelMask_)
logger_.log (module_, instance_, curLevel_, buffer_);
buffer_.erase ();
}
@@ -97,13 +142,13 @@ Log::overflow (int c)
{
buffer_ += c;
}
- return c;*/
+ return c;
}
/// Partie commune des constructeurs.
void
Log::init (void)
{
- maxLevel_ = logger_.maxLevel (module_);
+ levelMask_ = logger_.getLevel (module_);
}
diff --git a/2004/i/nono/src/logger/log.h b/2004/i/nono/src/logger/log.h
index d2add57..2d919cd 100644
--- a/2004/i/nono/src/logger/log.h
+++ b/2004/i/nono/src/logger/log.h
@@ -30,26 +30,26 @@
class Logger;
-class Log : public std::ostream, protected std::streambuf
+class Log : protected std::streambuf, public std::ostream
{
public:
enum Level
{
- none,
- fatal,
- error,
- warning,
- info,
- debug,
- verydebug,
- nbLevel
+ none = 0x00,
+ fatal = 0x01,
+ error = 0x02,
+ warning = 0x04,
+ info = 0x08,
+ debug = 0x10,
+ verydebug = 0x20,
+ levelReserved = 0xffff
};
private:
std::string module_;
std::string instance_;
std::string buffer_;
static Logger logger_;
- Level maxLevel_;
+ Level levelMask_;
Level curLevel_;
public:
/// Constructeur.
@@ -62,6 +62,8 @@ class Log : public std::ostream, protected std::streambuf
static void toString (Level level, std::string &s);
/// Traduit le niveau de log.
static Level toLevel (const std::string &level);
+ /// Traduit le masque de niveau de log.
+ static Level toLevelMask (const std::string &level);
protected:
/// Appellé lorsque le tampon streambuf déborde.
int overflow (int c);
diff --git a/2004/i/nono/src/logger/logger.cc b/2004/i/nono/src/logger/logger.cc
index 60cb013..4f2c4b7 100644
--- a/2004/i/nono/src/logger/logger.cc
+++ b/2004/i/nono/src/logger/logger.cc
@@ -39,8 +39,8 @@ Logger::Logger (void)
{
rc.getId (module);
rc.getId (level);
- l = Log::toLevel (level);
- if (l != Log::nbLevel)
+ l = Log::toLevelMask (level);
+ if (l != Log::levelReserved)
logLevels_[module] = l;
else
rc.throwError ("Log level expected.\n");
@@ -53,9 +53,9 @@ Logger::Logger (void)
}
}
-/// Récupère le level maximum pour un module.
+/// Récupère le log level pour un module.
Log::Level
-Logger::maxLevel (const std::string &module) const
+Logger::getLevel (const std::string &module) const
{
LogLevels::const_iterator i;
i = logLevels_.find (module);
diff --git a/2004/i/nono/src/logger/logger.h b/2004/i/nono/src/logger/logger.h
index ced2765..6cc7f7d 100644
--- a/2004/i/nono/src/logger/logger.h
+++ b/2004/i/nono/src/logger/logger.h
@@ -36,8 +36,8 @@ class Logger
public:
/// Constructeur par default.
Logger (void);
- /// Récupère le level maximum pour un module.
- Log::Level maxLevel (const std::string &module) const;
+ /// Récupère le log level pour un module.
+ Log::Level getLevel (const std::string &module) const;
/// Loggue un message.
void log (const std::string &module, const std::string &instance,
Log::Level level, const std::string &msg);
diff --git a/2004/i/nono/src/motor/asserv.cc b/2004/i/nono/src/motor/asserv.cc
index 2107908..e0ebd37 100644
--- a/2004/i/nono/src/motor/asserv.cc
+++ b/2004/i/nono/src/motor/asserv.cc
@@ -308,9 +308,11 @@ Asserv::handleStatMotor (void)
return;
}
char side = inBuf_[2];
- //int vacc = getSignedChar (inBuf_ + 2 + 1);
+ int vacc = getSignedChar (inBuf_ + 2 + 1);
int e = getSignedShort (inBuf_ + 2 + 1 + 2 + 1);
int pwm = getSignedShort (inBuf_ + 2 + 1 + 2 + 1 + 4 + 1);
+ log_ (Log::verydebug) << "stat motor " << side << ' ' << vacc << ' ' << e
+ << ' ' << pwm << std::endl;
}
/// Traite un message du compteur.
@@ -354,6 +356,14 @@ Asserv::handleCounter (void)
countRight_ = r;
}
+/// Décode un mot signé (1 octets).
+int
+Asserv::getSignedChar (const char *s) const
+{
+ return (signed char) (hex2digit (s[0]) << 4
+ | hex2digit (s[1]) << 0);
+}
+
/// Décode un mot signé (2 octets).
int
Asserv::getSignedShort (const char *s) const
diff --git a/2004/i/nono/src/motor/asserv.h b/2004/i/nono/src/motor/asserv.h
index cfda523..e95a3c7 100644
--- a/2004/i/nono/src/motor/asserv.h
+++ b/2004/i/nono/src/motor/asserv.h
@@ -106,6 +106,8 @@ class Asserv
void handleStatMotor (void);
/// Traite un message du compteur.
void handleCounter (void);
+ /// Décode un mot signé (1 octets).
+ int getSignedChar (const char *s) const;
/// Décode un mot signé (2 octets).
int getSignedShort (const char *s) const;
};
diff --git a/2004/i/nono/src/motor/motor.cc b/2004/i/nono/src/motor/motor.cc
index 9fc7c6c..f41dda8 100644
--- a/2004/i/nono/src/motor/motor.cc
+++ b/2004/i/nono/src/motor/motor.cc
@@ -25,7 +25,6 @@
#include "motor.h"
#include "config/config.h"
-#include <iostream>
#include <math.h>
// Pointeur vers l'instance unique.
@@ -33,7 +32,7 @@ Motor *Motor::instance_ = 0;
/// Constructeur.
Motor::Motor (void)
- : asserv_ (*this),
+ : asserv_ (*this), log_ ("motor"),
s_ (1.0), vs_ (1.0), as_ (1.0),
movement_ (0), maxSpeed_ (0x40), minSpeed_ (0x04)
{
@@ -121,20 +120,20 @@ Motor::waitStopped (void)
} while (!stopped ());
}
-/// Paramètre la vitesse des moteurs.
+/// Paramètre la vitesse des moteurs (-1..+1).
void
Motor::speed (double l, double r)
{
- int vL = (int) (l / vs_);
- int vR = (int) (r / vs_);
+ int vL = (int) ((vs_ > 0 ? l : -l) * maxSpeed_);
+ int vR = (int) ((vs_ > 0 ? r : -r) * maxSpeed_);
// 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_);
+ vL = (int) ((vs_ > 0 ? l : -l) * maxSpeed_);
+ vR = (int) ((vs_ > 0 ? r : -r) * maxSpeed_);
}
// Teste le dépassement de vitesse maximal.
if (vL > maxSpeed_)
@@ -158,7 +157,8 @@ Motor::speed (double l, double r)
vL = (int) (l / r * vR);
}
// Envois la commande.
-std::cout << "motor speed " << l << ' ' << r << ' ' << vL << ' ' << vR << std::endl;;
+ log_ (Log::debug) << "speed " << l << ' ' << r << ' ' << vL << ' '
+ << vR << std::endl;;
asserv_.speed (vL, vR);
}
@@ -177,7 +177,7 @@ 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;
+ log_ (Log::debug) << "goto arc " << l << ' ' << r;
if (l > 0.0)
l = sqrt (2.0 * asserv_.getAccel () * as_ * l);
else
@@ -186,7 +186,7 @@ std::cout << "goto arc " << l << ' ' << r;
r = sqrt (2.0 * asserv_.getAccel () * as_ * r);
else
r = -sqrt (2.0 * asserv_.getAccel () * as_ * -r);
-std::cout << ' ' << l << ' ' << r << std::endl;;
+ 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 7673534..fca17c4 100644
--- a/2004/i/nono/src/motor/motor.h
+++ b/2004/i/nono/src/motor/motor.h
@@ -37,6 +37,8 @@ class Motor : public AsservTracker
Asserv asserv_;
/// Suiveur de position.
Tracker tracker_;
+ // Système de log.
+ Log log_;
/// Pointeur vers l'instance unique.
static Motor *instance_;
/// Echelles (mm/unit, mm/s.unit, mm/s^2.unit).
@@ -72,7 +74,7 @@ class Motor : public AsservTracker
void waitStopped (void);
/// Récupère l'object tracker.
const Tracker &getTracker (void) const { return tracker_; }
- /// Paramètre la vitesse des moteurs.
+ /// Paramètre la vitesse des moteurs (-1..+1).
void speed (double l, double r);
/// Va vers un point, renvois faux si atteind.
bool goTo (double x, double y, double eps = 5.0);
diff --git a/2004/i/nono/src/motor/movement.h b/2004/i/nono/src/motor/movement.h
index 5a3deaf..e6f5e95 100644
--- a/2004/i/nono/src/motor/movement.h
+++ b/2004/i/nono/src/motor/movement.h
@@ -31,6 +31,8 @@ class Tracker;
class Asserv;
class Motor;
+/// Classe de base pour un mouvement. Classe gérant l'asservissement de la
+/// position.
class Movement
{
protected:
@@ -38,13 +40,13 @@ class Movement
Asserv *a_;
Motor *m_;
public:
- // Destructeur.
+ /// Destructeur.
virtual ~Movement (void) { }
- // Initialise le mouvement, appelé juste quand le mouvement est mis en
- // service.
+ /// Initialise le mouvement, appelé juste quand le mouvement est mis en
+ /// service.
virtual void init (const Tracker &t, Asserv &a, Motor &m)
{ t_ = &t; a_ = &a; m_ = &m; }
- // Controlle la vitesse, retourne faux si mouvement terminé.
+ /// Controlle la vitesse, retourne faux si mouvement terminé.
virtual bool control (void) { return false; }
};
diff --git a/2004/i/nono/src/motor/movement_basic.cc b/2004/i/nono/src/motor/movement_basic.cc
index 2b1cb6e..656ca35 100644
--- a/2004/i/nono/src/motor/movement_basic.cc
+++ b/2004/i/nono/src/motor/movement_basic.cc
@@ -27,15 +27,17 @@
#include "asserv.h"
#include "motor.h"
-// Constructeur.
+/// Constructeur.
+/// speed : vitesse (-1..+1).
+/// lenght : distance (mm, > 0)
MovementBasic::MovementBasic (double speed, double lenght)
: speed_ (speed), lenght2_ (lenght * lenght),
sX_ (0.0), sY_ (0.0)
{
}
-// Initialise le mouvement, appelé juste quand le mouvement est mis en
-// service.
+/// Initialise le mouvement, appelé juste quand le mouvement est mis en
+/// service.
void
MovementBasic::init (const Tracker &t, Asserv &a, Motor &m)
{
@@ -44,7 +46,7 @@ MovementBasic::init (const Tracker &t, Asserv &a, Motor &m)
sY_ = t.getY ();
}
-// Controlle la vitesse, retourne faux si mouvement terminé.
+/// Controlle la vitesse, retourne faux si mouvement terminé.
bool
MovementBasic::control (void)
{
diff --git a/2004/i/nono/src/motor/movement_basic.h b/2004/i/nono/src/motor/movement_basic.h
index 4c9a899..151d808 100644
--- a/2004/i/nono/src/motor/movement_basic.h
+++ b/2004/i/nono/src/motor/movement_basic.h
@@ -26,21 +26,24 @@
// }}}
#include "movement.h"
+/// Mouvemenet de base, avance tout droit jusqu'a avoir parcouru une distance.
class MovementBasic : public Movement
{
- // Vitesse.
+ /// Vitesse.
double speed_;
- // Distance au carré.
+ /// Distance au carré.
double lenght2_;
- // Position de départ.
+ /// Position de départ.
double sX_, sY_;
public:
- // Constructeur.
+ /// Constructeur.
+ /// speed : vitesse (-1..+1).
+ /// lenght : distance (mm, > 0)
MovementBasic (double speed, double lenght);
- // Initialise le mouvement, appelé juste quand le mouvement est mis en
- // service.
+ /// Initialise le mouvement, appelé juste quand le mouvement est mis en
+ /// service.
void init (const Tracker &t, Asserv &a, Motor &m);
- // Controlle la vitesse, retourne faux si mouvement terminé.
+ /// Controlle la vitesse, retourne faux si mouvement terminé.
bool control (void);
};
diff --git a/2004/i/nono/src/motor/movement_goto.cc b/2004/i/nono/src/motor/movement_goto.cc
index 794463c..b96526a 100644
--- a/2004/i/nono/src/motor/movement_goto.cc
+++ b/2004/i/nono/src/motor/movement_goto.cc
@@ -54,14 +54,15 @@ MovementGotoParam::MovementGotoParam (void)
}
}
-// Constructeur.
+/// Constructeur.
+/// (x, y) : position d'arrivée (mm).
MovementGoto::MovementGoto (double x, double y)
: dX_ (x), dY_ (y),
il_ (0.0), ia_ (0.0), lel_ (0.0), lea_ (0.0)
{
}
-// Controlle la vitesse, retourne faux si mouvement terminé.
+/// Controlle la vitesse, retourne faux si mouvement terminé.
bool
MovementGoto::control (void)
{
diff --git a/2004/i/nono/src/motor/movement_goto.h b/2004/i/nono/src/motor/movement_goto.h
index 721d481..375946f 100644
--- a/2004/i/nono/src/motor/movement_goto.h
+++ b/2004/i/nono/src/motor/movement_goto.h
@@ -26,6 +26,7 @@
// }}}
#include "movement.h"
+/// Paramètres d'un mouvement Goto.
class MovementGotoParam
{
// Epsilon.
@@ -46,20 +47,23 @@ class MovementGotoParam
friend class MovementGoto;
};
+/// Mouvement vers un point. Aucune garantie n'est fournie quand à la
+/// direction finale.
class MovementGoto : public Movement
{
- // Position d'arrivée.
+ /// Position d'arrivée.
double dX_, dY_;
- // Integrales.
+ /// Integrales.
double il_, ia_;
- // Dernière valeurs d'erreur.
+ /// Dernière valeurs d'erreur.
double lel_, lea_;
- // Paramètres d'asservissement.
+ /// Paramètres d'asservissement.
static MovementGotoParam param_;
public:
- // Constructeur.
+ /// Constructeur.
+ /// (x, y) : position d'arrivée (mm).
MovementGoto (double x, double y);
- // Controlle la vitesse, retourne faux si mouvement terminé.
+ /// Controlle la vitesse, retourne faux si mouvement terminé.
bool control (void);
};
diff --git a/2004/i/nono/src/motor/movement_rotation.cc b/2004/i/nono/src/motor/movement_rotation.cc
new file mode 100644
index 0000000..2f58e45
--- /dev/null
+++ b/2004/i/nono/src/motor/movement_rotation.cc
@@ -0,0 +1,84 @@
+// movement_rotation.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_rotation.h"
+#include "tracker.h"
+#include "motor.h"
+#include "config/config.h"
+
+MovementRotationParam MovementRotation::param_;
+
+/// Constructeur, charge les paramètres depuis la Config.
+MovementRotationParam::MovementRotationParam (void)
+ : eps_ (0.01),
+ kpa_ (1.0), kia_ (0.0), is_ (1000.0),
+ kda_ (0.0)
+{
+ // Lit la conf.
+ Config rc ("rc/movement/rotation");
+ while (!rc.eof ())
+ {
+ if (!(
+ rc.get ("epsilon", eps_) ||
+ rc.get ("kpa", kpa_) ||
+ rc.get ("kia", kia_) ||
+ rc.get ("is", is_) ||
+ rc.get ("kda", kda_)
+ ))
+ rc.noId ();
+ }
+}
+
+/// Constructeur.
+/// a : angle d'arrivé (rad).
+MovementRotation::MovementRotation (double a)
+ : dA_ (a),
+ ia_ (0.0), lea_ (0.0)
+{
+}
+
+/// Controlle la vitesse, retourne faux si mouvement terminé.
+bool
+MovementRotation::control (void)
+{
+ // Calcule l'erreur.
+ double ea;
+ if (!t_->computeAngleError (dA_, ea, param_.eps_))
+ {
+ return false;
+ }
+std::cout << "movement rotation error " << ea << std::endl;
+ // Calcule les intégrales saturées.
+ ia_ += ea;
+ if (ia_ > param_.is_) ia_ = param_.is_;
+ else if (ia_ < -param_.is_) ia_ = -param_.is_;
+ // Commande les moteurs.
+ double a = param_.kpa_ * (ea + param_.kia_ * ia_
+ + param_.kda_ * (ea - lea_));
+ m_->speed (a, -a);
+ // Retiens l'erreur pour la dérivée.
+ lea_ = ea;
+ return true;
+}
+
diff --git a/2004/i/nono/src/motor/movement_rotation.h b/2004/i/nono/src/motor/movement_rotation.h
new file mode 100644
index 0000000..4530ec3
--- /dev/null
+++ b/2004/i/nono/src/motor/movement_rotation.h
@@ -0,0 +1,67 @@
+#ifndef movement_rotation_h
+#define movement_rotation_h
+// movement_rotation.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"
+
+/// Paramètres de MovementRotation.
+class MovementRotationParam
+{
+ // Epsilon.
+ double eps_;
+ // Coefficient proportionnel.
+ double kpa_;
+ // Coefficient intégral.
+ double kia_;
+ // Maximum de saturation de l'integralle.
+ double is_;
+ // Coefficient de dérivée.
+ double kda_;
+ public:
+ /// Constructeur, charge les paramètres depuis la Config.
+ MovementRotationParam (void);
+ friend class MovementRotation;
+};
+
+/// Mouvement de rotation, permet d'atteindre un angle.
+class MovementRotation : public Movement
+{
+ /// Angle d'arrivée.
+ double dA_;
+ /// Integrale.
+ double ia_;
+ /// Dernière valeurs d'erreur.
+ double lea_;
+ /// Paramètres d'asservissement.
+ static MovementRotationParam param_;
+ public:
+ /// Constructeur.
+ /// a : angle d'arrivé (rad).
+ MovementRotation (double a);
+ /// Controlle la vitesse, retourne faux si mouvement terminé.
+ bool control (void);
+};
+
+#endif // movement_rotation_h
diff --git a/2004/i/nono/src/motor/test_asserv.cc b/2004/i/nono/src/motor/test_asserv.cc
index 7ac096c..4aa05ae 100644
--- a/2004/i/nono/src/motor/test_asserv.cc
+++ b/2004/i/nono/src/motor/test_asserv.cc
@@ -36,6 +36,7 @@ class AsservTrackerC : public AsservTracker
{
int s;
public:
+ AsservTrackerC (void) : s (0) { }
// Appelée lors d'une mise à jour des compteurs.
virtual void updateCounter (int l, int r) { s += l; }
// Affiche.
@@ -46,6 +47,22 @@ class AsservTrackerC : public AsservTracker
{ s = 0; }
};
+void
+syntax (void)
+{
+ std::cout <<
+ "test_asserv - teste la communication avec la carte "
+ "d'asservissement.\n"
+ " s stop\n"
+ " z rezet\n"
+ " g go (active l'asservissement)\n"
+ " G ungo (désactive l'asservissement)\n"
+ " v <vg> <vd> vitesse gauche (vg) et droite (vd) en unité de la\n"
+ " carte d'asservissement\n"
+ " <timeout> attend un certain temps (qui augmente avec timeout)\n"
+ " ? cet ecran d'aide" << std::endl;
+}
+
int
main (int argc, char **argv)
{
@@ -89,9 +106,17 @@ main (int argc, char **argv)
cout << "speed " << vg << ' ' << vd << endl;
m.speed (vg, vd);
break;
+ case '?':
+ syntax ();
+ return 0;
default:
int s;
s = atoi (argv[i++]);
+ if (s == 0)
+ {
+ syntax ();
+ return 1;
+ }
cout << "sleep " << s << endl;
for (int j = 0; j < s * 1000 / 50; ++j)
{
diff --git a/2004/i/nono/src/motor/test_motor.cc b/2004/i/nono/src/motor/test_motor.cc
index 2f5f893..5ad15a2 100644
--- a/2004/i/nono/src/motor/test_motor.cc
+++ b/2004/i/nono/src/motor/test_motor.cc
@@ -34,6 +34,22 @@
using namespace std;
+void
+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ées\n"
+ " <timeout> attend un certain temps (qui augmente avec timeout)\n"
+ " ? cet ecran d'aide" << std::endl;
+}
+
int
main (int argc, char **argv)
{
@@ -94,13 +110,22 @@ main (int argc, char **argv)
if (i >= argc) break;
dA = atof (argv[i++]);
cout << "rotation " << dA << endl;
- Movement *mov = new MovementRotation (dA);
+ Movement *mov = new MovementRotation (dA * 2 * M_PI /
+ 360);
m.setMovement (mov);
}
break;
+ case '?':
+ syntax ();
+ return 0;
default:
int s;
s = atoi (argv[i++]);
+ if (s == 0)
+ {
+ syntax ();
+ return 1;
+ }
cout << "sleep " << s << endl;
for (int j = 0; j < s * 1000 / 50; ++j)
{
diff --git a/2004/i/nono/src/motor/tracker.cc b/2004/i/nono/src/motor/tracker.cc
index 25f6f22..e5779da 100644
--- a/2004/i/nono/src/motor/tracker.cc
+++ b/2004/i/nono/src/motor/tracker.cc
@@ -25,13 +25,12 @@
#include "tracker.h"
#include "config/config.h"
-#include <iostream>
#include <math.h>
-// Constructeur.
+/// Constructeur.
Tracker::Tracker (void)
: posX_ (0.0), posY_ (0.0), angle_ (0.0),
- f_ (10.0), zero_ (0)
+ f_ (10.0), zero_ (0), log_ ("tracker")
{
// Lit la conf.
Config rc ("rc/tracker");
@@ -47,12 +46,12 @@ Tracker::Tracker (void)
}
}
-// Destructeur.
+/// Destructeur.
Tracker::~Tracker (void)
{
}
-// Set the position.
+/// Set the position.
void
Tracker::setPos (double x, double y, double angle)
{
@@ -61,7 +60,7 @@ Tracker::setPos (double x, double y, double angle)
angle_ = angle;
}
-// Get the position.
+/// Get the position.
void
Tracker::getPos (double &x, double &y, double &angle) const
{
@@ -70,8 +69,24 @@ Tracker::getPos (double &x, double &y, double &angle) const
angle = angle_;
}
-// Calcule l'erreur linéaire et angulaire pour atteindre un point,
-// retourne faux si atteind.
+/// Récupère l'angle entre l'angle courant et a normalisé entre -pi et pi.
+double
+Tracker::getAngleDiff (double a) const
+{
+ double diff = angle_ - a;
+ while (diff > M_PI)
+ diff -= M_PI;
+ while (diff < -M_PI)
+ diff += M_PI;
+ return diff;
+}
+
+/// Calcule l'erreur linéaire et angulaire pour atteindre un point,
+/// retourne faux si atteind.
+/// (x, y) : point à atteindre (mm).
+/// el, ea : erreur linéaire et angulaire (-1..+1).
+/// eps : distance en dessous de laquelle on considère qu'on est arrivé (mm).
+/// dist : Distance maximale du point de référence (mm).
bool
Tracker::computeError (double x, double y, double &el, double &ea, double eps,
double dist) const
@@ -95,6 +110,9 @@ Tracker::computeError (double x, double y, double &el, double &ea, double eps,
eX = eX / ds * dist;
eY = eY / ds * dist;
}
+ // Ramène sur -1..+1
+ eX /= dist;
+ eY /= dist;
// Calcule l'erreur linéaire (projection sur la direction courante).
el = eX * cos (angle_) + eY * sin (angle_);
// Calcule l'erreur angulaire (projection sur la direction
@@ -103,22 +121,27 @@ Tracker::computeError (double x, double y, double &el, double &ea, double eps,
return true;
}
-// Calcule l'erreur angulaire pour atteindre un angle, retourne faux si
-// atteind.
+/// Calcule l'erreur angulaire pour atteindre un angle, retourne faux si
+/// atteind.
+/// a : angle à atteindre (rad).
+/// ea : erreur angulaire (-1..+1).
+/// eps : angle en dessous duquel on considère qu'on est arrivé (rad).
bool
Tracker::computeAngleError (double a, double &ea, double eps) const
{
- ea = a - angle_;
- if (ea < eps)
+ ea = getAngleDiff (a);
+ if (-eps < ea && ea < eps)
{
ea = 0.0;
return false;
}
+ ea *= 1.0 / M_PI;
return true;
}
-// Calcule la longueur de l'arc gauche et droite pour atteindre un point.
-// EPS précise la distance minimale. Renvois faux si atteind.
+/// Calcule la longueur de l'arc gauche et droite pour atteindre un point.
+/// EPS précise la distance minimale. Renvois faux si atteind.
+/// \deprecated Pas vraiment l'algo qui faut.
bool
Tracker::computeArcs (double x, double y, double &l, double &r, double eps)
const
@@ -130,7 +153,7 @@ Tracker::computeArcs (double x, double y, double &l, double &r, double eps)
if (d < eps)
{
l = r = 0.0;
-std::cout << "compute arc eps" << std::endl;
+ log_ (Log::verydebug) << "compute arc eps" << std::endl;
return false;
}
// Calcule l'angle entre la direction courante et la direction du point à
@@ -143,24 +166,25 @@ std::cout << "compute arc eps" << std::endl;
if (dX * vY + dY * -vX < 0.0)
{
l = r = 0;
-std::cout << "compute arc back" << std::endl;
+ log_ (Log::verydebug) << "compute arc back" << std::endl;
return false;
}
// Calcul de l'angle par produit scalaire.
double s = dX * vX + dY * vY;
double dA = asin (s / d); // sin (a) = cos (a + pi/2)
-std::cout << "compute arc dA " << dA << " d " << d << std::endl;
+ log_ (Log::verydebug) << "compute arc dA " << dA << " d " << d <<
+ std::endl;
// Si l'angle est trops petit, on va tout droit, sinon /0.
if (dA > -0.0001 && dA < 0.0001)
{
l = r = d;
-std::cout << "compute arc small angle" << std::endl;
+ log_ (Log::verydebug) << "compute arc small angle" << std::endl;
}
// Si l'angle est trops grand, on ne fait pas le tour du monde.
else if (dA > M_PI_2 || dA < -M_PI_2)
{
l = r = 0;
-std::cout << "compute arc big angle" << std::endl;
+ log_ (Log::verydebug) << "compute arc big angle" << std::endl;
return false;
}
else
@@ -171,18 +195,19 @@ std::cout << "compute arc big angle" << std::endl;
if (ro < 0.25 * f_ && ro > 0.25 * f_)
{
l = r = 0.0;
-std::cout << "compute arc small ro " << ro << std::endl;
+ log_ (Log::verydebug) << "compute arc small ro " << ro <<
+ std::endl;
return false;
}
// L'angle du déplacement est le même que dA.
l = (ro - 0.5 * f_) * dA;
r = (ro + 0.5 * f_) * dA;
-std::cout << "compute arc ro " << ro << std::endl;
+ log_ (Log::verydebug) << "compute arc ro " << ro << std::endl;
}
return true;
}
-// Met à jour la position.
+/// Met à jour la position.
void
Tracker::update (double dL, double dR)
{
@@ -200,7 +225,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);
-//std::cout << "tracker 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)
{
@@ -210,7 +236,8 @@ Tracker::update (double dL, double dR)
posX_ += dS * cos (a);
posY_ += dS * sin (a);
angle_ += dA;
-//std::cout << "tracker small dX " << dS * cos (a) << " dY " << dS * sin (a) << std::endl;
+ log_ (Log::verydebug) << "small dX " << dS * cos (a) << " dY "
+ << dS * sin (a) << std::endl;
}
else
{
@@ -221,12 +248,15 @@ Tracker::update (double dL, double dR)
angle_ += dA;
posX_ += (sin (angle_) - sin (oldA)) * dS / dA;
posY_ += (cos (oldA) - cos (angle_)) * dS / dA;
-//std::cout << "tracker big dX " << -(sin (oldA) - sin (angle_)) * dS / dA << " dY " << -(cos (oldA) - cos (angle_)) * dS / dA << " R " << dS / dA << std::endl;
+ log_ (Log::verydebug) << "big dX " <<
+ -(sin (oldA) - sin (angle_)) * dS / dA <<
+ " dY " << -(cos (oldA) - cos (angle_)) * dS / dA <<
+ " R " << dS / dA << std::endl;
}
-std::cout << "update pos " << *this << std::endl;
+ log_ (Log::info) << "update pos " << *this << std::endl;
}
-// Affiche la position.
+/// Affiche la position.
std::ostream &
operator<< (std::ostream &os, const Tracker &t)
{
diff --git a/2004/i/nono/src/motor/tracker.h b/2004/i/nono/src/motor/tracker.h
index 2674782..63c58b2 100644
--- a/2004/i/nono/src/motor/tracker.h
+++ b/2004/i/nono/src/motor/tracker.h
@@ -24,46 +24,65 @@
// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
//
// }}}
+#include "logger/log.h"
#include <iostream>
+/// Classe de suivi des coordonnée et de calcul d'erreur.
class Tracker
{
- // Position et angle (mm et rad).
+ /// Position et angle (mm et rad).
double posX_, posY_, angle_;
- // Distance entre les deux roues (mm).
+ /// Distance entre les deux roues (mm).
double f_;
- // Nombre d'updates nuls consécutifs.
+ /// Nombre d'updates nuls consécutifs.
int zero_;
+ // Système de log.
+ mutable Log log_;
public:
- // Constructeur.
+ /// Constructeur.
Tracker (void);
- // Destructeur.
+ /// Destructeur.
~Tracker (void);
- // Set the position.
+ /// Set the position.
void setPos (double x, double y, double angle);
- // Get the position.
+ /// Get the position.
void getPos (double &x, double &y, double &angle) const;
+ /// Get X position.
double getX (void) const { return posX_; }
+ /// Get Y position.
double getY (void) const { return posY_; }
+ /// Get angle.
double getAngle (void) const { return angle_; }
+ /// Retourne vrai si arrété.
bool stopped (void) { return zero_ > 10; }
- // Calcule l'erreur linéaire et angulaire pour atteindre un point,
- // retourne faux si atteind.
+ /// Récupère l'angle entre l'angle courant et a normalisé entre -pi et pi.
+ double getAngleDiff (double a) const;
+ /// Calcule l'erreur linéaire et angulaire pour atteindre un point,
+ /// retourne faux si atteind.
+ /// (x, y) : point à atteindre (mm).
+ /// el, ea : erreur linéaire et angulaire (-1..+1).
+ /// eps : distance en dessous de laquelle on considère qu'on est arrivé
+ /// (mm).
+ /// dist : Distance maximale du point de référence (mm).
bool computeError (double x, double y, double &el, double &ea, double eps,
double dist) const;
- // Calcule l'erreur angulaire pour atteindre un angle, retourne faux si
- // atteind.
+ /// Calcule l'erreur angulaire pour atteindre un angle, retourne faux si
+ /// atteind.
+ /// a : angle à atteindre (rad).
+ /// ea : erreur angulaire (-1..+1).
+ /// eps : angle en dessous duquel on considère qu'on est arrivé (rad).
bool computeAngleError (double a, double &ea, double eps) const;
- // Calcule la longueur de l'arc gauche et droite pour atteindre un point.
- // EPS précise la distance minimale. Renvois faux si atteind.
+ /// Calcule la longueur de l'arc gauche et droite pour atteindre un point.
+ /// EPS précise la distance minimale. Renvois faux si atteind.
+ /// \deprecated Pas vraiment l'algo qui faut.
bool computeArcs (double x, double y, double &l, double &r, double eps)
const;
- // Met à jour la position.
+ /// Met à jour la position.
void update (double dL, double dR);
};
-// Affiche la position.
+/// Affiche la position.
std::ostream &operator<< (std::ostream &os, const Tracker &t);
#endif // tracker_h
diff --git a/2004/i/nono/src/serial/serial.h b/2004/i/nono/src/serial/serial.h
index 808c234..e3b03de 100644
--- a/2004/i/nono/src/serial/serial.h
+++ b/2004/i/nono/src/serial/serial.h
@@ -54,6 +54,9 @@ class Serial
void write (const void *buf, size_t size) { sb_->write (buf, size); }
/// Ecrit un caractère.
void putchar (int c) { sb_->putchar (c); }
+ /// Attend que des caractères soient disponibles pendant un delay en
+ /// millisecondes.
+ bool wait (int timeout = -1) { return sb_->wait (timeout); }
};
#endif // serial_h
diff --git a/2004/i/nono/src/serial/serial_base.cc b/2004/i/nono/src/serial/serial_base.cc
index 4815c67..c43bb59 100644
--- a/2004/i/nono/src/serial/serial_base.cc
+++ b/2004/i/nono/src/serial/serial_base.cc
@@ -23,6 +23,10 @@
//
// }}}
#include "serial_base.h"
+#include "utils/errno_exception.h"
+
+#include <sys/types.h>
+#include <sys/time.h>
/// Constructeur.
SerialBase::SerialBase (void)
@@ -70,3 +74,40 @@ SerialBase::putchar (int c)
write (&c, 1);
}
+/// Attend que des caractères soient disponibles pendant un delay en
+/// millisecondes.
+bool
+SerialBase::wait (int timeout/*-1*/)
+{
+ fd_set fds;
+ int r;
+ // Setup fd_set.
+ FD_ZERO (&fds);
+ FD_SET (fd_, &fds);
+ if (timeout == -1)
+ {
+ // Without timeout.
+ r = select (FD_SETSIZE, &fds, 0, 0, 0);
+ }
+ else
+ {
+ // With timeout.
+ struct timeval tv;
+ tv.tv_sec = timeout;
+ tv.tv_usec = 0;
+ r = select (FD_SETSIZE, &fds, 0, 0, &tv);
+ }
+ switch (r)
+ {
+ case -1:
+ // Error.
+ throw errno_exception (errno);
+ case 0:
+ // Timeout.
+ return false;
+ default:
+ // Data availlable.
+ return true;
+ }
+}
+
diff --git a/2004/i/nono/src/serial/serial_base.h b/2004/i/nono/src/serial/serial_base.h
index 8cf6bd3..2c092b3 100644
--- a/2004/i/nono/src/serial/serial_base.h
+++ b/2004/i/nono/src/serial/serial_base.h
@@ -55,6 +55,9 @@ class SerialBase
void write (const void *buf, size_t size);
/// Ecrit un caractère.
void putchar (int c);
+ /// Attend que des caractères soient disponibles pendant un delay en
+ /// millisecondes.
+ bool wait (int timeout = -1);
};
#endif // serial_base_h
diff --git a/2004/i/nono/src/utils/errno_exception.h b/2004/i/nono/src/utils/errno_exception.h
new file mode 100644
index 0000000..436a4c5
--- /dev/null
+++ b/2004/i/nono/src/utils/errno_exception.h
@@ -0,0 +1,25 @@
+#ifndef errno_exception_h
+#define errno_exception_h
+// errno_exception.h
+
+#include <string>
+#include <exception>
+#include <cstring>
+#include <errno.h>
+
+/// Exception lancé lorsqu'une erreur est retournée par la libc.
+class errno_exception : public std::exception
+{
+ int errno_;
+ std::string what_;
+ public:
+ errno_exception (const std::string &desc, int errno__)
+ : errno_ (errno__), what_ (desc + ": " + strerror (errno__)) { }
+ errno_exception (int errno__)
+ : errno_ (errno__), what_ (strerror (errno__)) { }
+ ~errno_exception (void) throw () { }
+ virtual const char* what () const throw () { return what_.c_str (); }
+ int getErrno (void) const { return errno_; }
+};
+
+#endif // errno_exception_h