summaryrefslogtreecommitdiff
path: root/2004/i/nono/src/motor
diff options
context:
space:
mode:
Diffstat (limited to '2004/i/nono/src/motor')
-rw-r--r--2004/i/nono/src/motor/Makefile.defs19
-rw-r--r--2004/i/nono/src/motor/asserv.cc385
-rw-r--r--2004/i/nono/src/motor/asserv.h106
-rw-r--r--2004/i/nono/src/motor/motor.cc216
-rw-r--r--2004/i/nono/src/motor/motor.h80
-rw-r--r--2004/i/nono/src/motor/movement.h51
-rw-r--r--2004/i/nono/src/motor/movement_basic.cc61
-rw-r--r--2004/i/nono/src/motor/movement_basic.h47
-rw-r--r--2004/i/nono/src/motor/movement_goto.cc68
-rw-r--r--2004/i/nono/src/motor/movement_goto.h55
-rw-r--r--2004/i/nono/src/motor/movement_types.h30
-rw-r--r--2004/i/nono/src/motor/test_asserv.cc113
-rw-r--r--2004/i/nono/src/motor/test_motor.cc113
-rw-r--r--2004/i/nono/src/motor/test_tracker.cc82
-rw-r--r--2004/i/nono/src/motor/tracker.cc230
-rw-r--r--2004/i/nono/src/motor/tracker.h66
16 files changed, 1722 insertions, 0 deletions
diff --git a/2004/i/nono/src/motor/Makefile.defs b/2004/i/nono/src/motor/Makefile.defs
new file mode 100644
index 0000000..5fda0ed
--- /dev/null
+++ b/2004/i/nono/src/motor/Makefile.defs
@@ -0,0 +1,19 @@
+TARGETS += test_motor test_asserv test_tracker
+LIBS += motor.a
+test_motor_SOURCES = test_motor.cc motor.a serial.a erreur.a config.a date.a
+test_asserv_SOURCES = test_asserv.cc motor.a serial.a erreur.a config.a date.a
+test_tracker_SOURCES = test_tracker.cc motor.a serial.a erreur.a config.a \
+ date.a
+motor_a_SOURCES = asserv.cc tracker.cc motor.cc movement_basic.cc \
+ movement_goto.cc
+
+test_motor: $(test_motor_SOURCES:%.cc=%.o)
+
+test_asserv: $(test_asserv_SOURCES:%.cc=%.o)
+
+test_tracker: $(test_tracker_SOURCES:%.cc=%.o)
+
+motor.a: ${motor_a_SOURCES:%.cc=motor.a(%.o)}
+
+clean_motor:
+ rm -f test_motor test_asserv test_tracker motor.a
diff --git a/2004/i/nono/src/motor/asserv.cc b/2004/i/nono/src/motor/asserv.cc
new file mode 100644
index 0000000..2d005b9
--- /dev/null
+++ b/2004/i/nono/src/motor/asserv.cc
@@ -0,0 +1,385 @@
+// asserv.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 "asserv.h"
+#include "config/config.h"
+#include "date/date.h"
+
+#include <iostream>
+
+// Table de conversion en hexa.
+const char *Asserv::hexaTbl_ = "0123456789abcdef";
+
+// Constructeur.
+Asserv::Asserv (AsservTracker &asservTracker)
+ : accel_ (-1), kp_ (-1), ki_ (-1), kd_ (-1),
+ statMotor_ (false), counter_ (false),
+ inBufSize_ (64), inBufPos_ (0), inBuf_ (0),
+ firstCounter_ (true),
+ countLeft_ (0), countRight_ (0),
+ asservTracker_ (asservTracker)
+{
+ // Lit la conf.
+ Config rc ("rc/asserv");
+ while (!rc.eof ())
+ {
+ if (rc.isId ("accel"))
+ {
+ rc.getId ();
+ rc >> accel_;
+ }
+ else if (rc.isId ("kp"))
+ {
+ rc.getId ();
+ rc >> kp_;
+ }
+ else if (rc.isId ("ki"))
+ {
+ rc.getId ();
+ rc >> ki_;
+ }
+ else if (rc.isId ("kd"))
+ {
+ rc.getId ();
+ rc >> kd_;
+ }
+ else if (rc.isId ("tty"))
+ {
+ rc.getId ();
+ rc >> ttyname_;
+ }
+ else if (rc.isId ("stats"))
+ {
+ rc.getId ();
+ rc >> statMotor_;
+ }
+ else if (rc.isId ("counter"))
+ {
+ rc.getId ();
+ rc >> counter_;
+ }
+ else if (rc.isId ("buffer"))
+ {
+ rc.getId ();
+ rc >> inBufSize_;
+ }
+ else rc.noId ();
+ }
+ // Ouvre le port série.
+ serial_.open (ttyname_.c_str ());
+ // Alloue la mémoire du tampon d'entrée.
+ inBuf_ = new char[inBufSize_];
+}
+
+// Destructeur.
+Asserv::~Asserv (void)
+{
+ // Envois le signal de reset.
+ send ('z');
+ waitOk ();
+ delete inBuf_;
+}
+
+// Reset la carte et envois les paramètres.
+void
+Asserv::reset (void)
+{
+ // Reset les données.
+ countLeft_ = countRight_ = 0;
+ inBufPos_ = 0;
+ while (!sendQueue_.empty ()) sendQueue_.pop ();
+ // Envois le signal de reset.
+ send ('z');
+ // Renvois les paramètres.
+ if (accel_ != -1)
+ setAccel (accel_);
+ if (kp_ != -1)
+ setKp (kp_);
+ if (ki_ != -1)
+ setKi (ki_);
+ if (kd_ != -1)
+ setKi (kd_);
+ if (statMotor_)
+ setStatMotor (statMotor_);
+ if (counter_)
+ setCounter (counter_);
+}
+
+// Active l'asservissement.
+void
+Asserv::go (bool fl)
+{
+ send ('g', fl);
+}
+
+// Stop !
+void
+Asserv::stop (void)
+{
+ send ('s');
+}
+
+// Réglage de la vitesse.
+void
+Asserv::speed (int l, int r)
+{
+ send ('v', l, r);
+}
+
+// Teste si l'émission est terminée.
+bool
+Asserv::ok (void)
+{
+ read ();
+ return sendQueue_.empty ();
+}
+
+// Attend que toute les émissions soit terminées.
+void
+Asserv::waitOk (void)
+{
+ while (!ok ()) Date::wait (1);
+}
+
+// Lit et traite les messages de la cartes.
+void
+Asserv::read (void)
+{
+ int c;
+ while ((c = serial_.getchar ()) != -1)
+ {
+ if (c == '\n' || c == '\r')
+ {
+ if (inBufPos_)
+ // Tampon plein.
+ handleMessage ();
+ }
+ else
+ {
+ if (c == '!' || inBufPos_ >= inBufSize_)
+ inBufPos_ = 0;
+ inBuf_[inBufPos_++] = c;
+ }
+ }
+}
+
+// Change les paramètres de la carte.
+void
+Asserv::setAccel (int accel)
+{
+ send ('a', accel);
+ accel_ = accel;
+}
+
+void
+Asserv::setKp (int kp)
+{
+ send ('p', kp);
+ kp_ = kp;
+}
+
+void
+Asserv::setKi (int ki)
+{
+ send ('i', ki);
+ ki_ = ki;
+}
+
+void
+Asserv::setKd (int kd)
+{
+ send ('d', kd);
+ kd_ = kd;
+}
+
+void
+Asserv::setStatMotor (bool fl)
+{
+ send ('m', fl);
+ statMotor_ = fl;
+}
+
+void
+Asserv::setCounter (bool fl = true)
+{
+ firstCounter_ = true;
+ send ('c', fl);
+ counter_ = fl;
+}
+
+// Envoie un message.
+void
+Asserv::send (char com)
+{
+ bool wasEmpty = ok ();
+ std::string s;
+ s += '!';
+ s += com;
+ s += '\r';
+ sendQueue_.push (s);
+ if (wasEmpty) sendLast ();
+}
+
+void
+Asserv::send (char com, bool fl)
+{
+ bool wasEmpty = ok ();
+ std::string s;
+ s += '!';
+ s += com;
+ s += fl ? '1' : '0';
+ s += '\r';
+ sendQueue_.push (s);
+ if (wasEmpty) sendLast ();
+}
+
+void
+Asserv::send (char com, int a1)
+{
+ bool wasEmpty = ok ();
+ std::string s;
+ s += '!';
+ s += com;
+ s += hexaTbl_[(a1 >> 4) & 0x0f];
+ s += hexaTbl_[a1 & 0x0f];
+ s += '\r';
+ sendQueue_.push (s);
+ if (wasEmpty) sendLast ();
+}
+
+void
+Asserv::send (char com, int a1, int a2)
+{
+ bool wasEmpty = ok ();
+ std::string s;
+ s += '!';
+ s += com;
+ s += hexaTbl_[(a1 >> 4) & 0x0f];
+ s += hexaTbl_[a1 & 0x0f];
+ s += hexaTbl_[(a2 >> 4) & 0x0f];
+ s += hexaTbl_[a2 & 0x0f];
+ s += '\r';
+ sendQueue_.push (s);
+ if (wasEmpty) sendLast ();
+}
+
+// Renvois le dernier message.
+void
+Asserv::sendLast (void)
+{
+ if (sendQueue_.empty ()) return;
+ std::string &s = sendQueue_.front ();
+std::cout << "send " << s << std::endl;
+ serial_.write (s.data (), s.size ());
+}
+
+// Traite un message.
+void
+Asserv::handleMessage (void)
+{
+inBuf_[inBufPos_] = 0;
+//std::cout << "recv " << inBuf_ << std::endl;
+ if (inBufPos_ > 1 && inBuf_[0] == '!')
+ {
+ switch (inBuf_[1])
+ {
+ case 'o':
+ case 'z':
+ // Ok.
+ if (!sendQueue_.empty ())
+ sendQueue_.pop ();
+ sendLast ();
+ break;
+ case 'e':
+ // Erreur, renvois la dernière commande.
+ sendLast ();
+ break;
+ case 'm':
+ // Recois des nouvelles stats.
+ // handleStatMotor ();
+ break;
+ case 'c':
+ // Recois une nouvelle valeur pour les compteurs.
+ handleCounter ();
+ break;
+ }
+ }
+ // Efface le tampon.
+ inBufPos_ = 0;
+}
+
+// Traite un message du compteur.
+void
+Asserv::handleCounter (void)
+{
+ if (inBufPos_ != 2 + 4 + 1 + 4)
+ {
+std::cout << "counter error" << std::endl;
+ // Mauvaise transmission.
+ return;
+ }
+ int l = getSignedShort (inBuf_ + 2);
+ int r = getSignedShort (inBuf_ + 2 + 4 + 1);
+ if (firstCounter_)
+ {
+ // Première valeur ignorée.
+ countLeft_ = l;
+ countRight_ = r;
+ firstCounter_ = false;
+ }
+ // Attention à l'overflow.
+//std::cout << "counter before " << countLeft_ << ' ' << countRight_ << ' ' << l << ' ' << r << std::endl;
+ if (l > 0x4000 && countLeft_ < -0x4000)
+ countLeft_ += 0x10000;
+ if (l < -0x4000 && countLeft_ > 0x4000)
+ countLeft_ -= 0x10000;
+ if (r > 0x4000 && countRight_ < -0x4000)
+ countRight_ += 0x10000;
+ if (r < -0x4000 && countRight_ > 0x4000)
+ countRight_ -= 0x10000;
+//std::cout << "counter after " << countLeft_ << ' ' << countRight_ << ' ' << l << ' ' << r << std::endl;
+ // Met à jour le tracker.
+ asservTracker_.updateCounter (l - countLeft_, r - countRight_);
+//std::cout << "tracker update " << l - countLeft_ << ' ' << r - countRight_ << std::endl;
+ // Retiens les anciennes valeurs.
+ countLeft_ = l;
+ countRight_ = r;
+}
+
+// Décode un mot signé (2 octets).
+int
+Asserv::getSignedShort (const char *s) const
+{
+ return (short) (hex2digit (s[0]) << 12
+ | hex2digit (s[1]) << 8
+ | hex2digit (s[2]) << 4
+ | hex2digit (s[3]));
+}
+
+// Décode un chiffre hexa.
+int
+Asserv::hex2digit (char c) const
+{
+ return (c >= '0' && c <= '9') ? c - '0' : c - 'a' + 10;
+}
diff --git a/2004/i/nono/src/motor/asserv.h b/2004/i/nono/src/motor/asserv.h
new file mode 100644
index 0000000..d6f1da1
--- /dev/null
+++ b/2004/i/nono/src/motor/asserv.h
@@ -0,0 +1,106 @@
+#ifndef asserv_h
+#define asserv_h
+// asserv.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 "serial/serial.h"
+
+#include <string>
+#include <queue>
+
+// Class dont on doit dériver pour pouvoir recevoir les informations de la
+// carte d'asservissement.
+class AsservTracker
+{
+ public:
+ // Appelée lors d'une mise à jour des compteurs.
+ virtual void updateCounter (int l, int r) { }
+};
+
+class Asserv
+{
+ // Ligne série.
+ Serial serial_;
+ std::string ttyname_;
+ // Paramètres.
+ int accel_, kp_, ki_, kd_;
+ bool statMotor_, counter_;
+ // File d'emmission.
+ std::queue<std::string> sendQueue_;
+ // Table de conversion en hexa.
+ static const char *hexaTbl_;
+ // Buffer de reception.
+ int inBufSize_, inBufPos_;
+ char *inBuf_;
+ // Anciènne valeur des compteur.
+ bool firstCounter_;
+ int countLeft_, countRight_;
+ // Objet interessé par les stats.
+ AsservTracker &asservTracker_;
+ public:
+ // Constructeur.
+ Asserv (AsservTracker &asservTracker);
+ // Destructeur.
+ ~Asserv (void);
+ // Reset la carte et envois les paramètres.
+ void reset (void);
+ // Active l'asservissement.
+ void go (bool fl = true);
+ // Stop !
+ void stop (void);
+ // Réglage de la vitesse.
+ void speed (int l, int r);
+ // Teste si l'émission est terminée.
+ bool ok (void);
+ // Attend que toute les émissions soit terminées.
+ 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);
+ void setStatMotor (bool fl = true);
+ void setCounter (bool fl = true);
+ protected:
+ // Envoie un message.
+ void send (char com);
+ void send (char com, bool fl);
+ void send (char com, int a1);
+ void send (char com, int a1, int a2);
+ // Renvois le dernier message.
+ void sendLast (void);
+ // Traite un message.
+ void handleMessage (void);
+ // Traite un message du compteur.
+ void handleCounter (void);
+ // Décode un mot signé (2 octets).
+ int getSignedShort (const char *s) const;
+ // Décode un chiffre hexa.
+ int hex2digit (char c) const;
+};
+
+#endif // asserv_h
diff --git a/2004/i/nono/src/motor/motor.cc b/2004/i/nono/src/motor/motor.cc
new file mode 100644
index 0000000..615511c
--- /dev/null
+++ b/2004/i/nono/src/motor/motor.cc
@@ -0,0 +1,216 @@
+// motor.cc
+// nono - programme du robot 2004. {{{
+//
+// Copyright (C) 2003 Nicolas Schodet
+//
+// Robot APB Team/Efrei 2004.
+// Web: http://assos.efrei.fr/robot/
+// Email: robot AT efrei DOT fr
+//
+// This program is free software; you can redistribute it and/or modify
+// it under the terms of the GNU General Public License as published by
+// the Free Software Foundation; either version 2 of the License, or
+// (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program; if not, write to the Free Software
+// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+//
+// }}}
+#include "motor.h"
+#include "config/config.h"
+
+#include <iostream>
+#include <math.h>
+
+// Pointeur vers l'instance unique.
+Motor *Motor::instance_ = 0;
+
+// Constructeur.
+Motor::Motor (void)
+ : asserv_ (*this),
+ s_ (1.0), vs_ (1.0), as_ (1.0),
+ movement_ (0), maxSpeed_ (0x40), minSpeed_ (0x04)
+{
+ // Mémorise le pointeur d'instance.
+ instance_ = this;
+ // Lit la conf.
+ Config rc ("rc/motor");
+ while (!rc.eof ())
+ {
+ if (rc.isId ("scale"))
+ {
+ rc.getId ();
+ rc >> s_;
+ }
+ else if (rc.isId ("speedscale"))
+ {
+ rc.getId ();
+ rc >> vs_;
+ }
+ else if (rc.isId ("accelscale"))
+ {
+ rc.getId ();
+ rc >> as_;
+ }
+ else if (rc.isId ("maxspeed"))
+ {
+ rc.getId ();
+ rc >> maxSpeed_;
+ }
+ else if (rc.isId ("minspeed"))
+ {
+ rc.getId ();
+ rc >> minSpeed_;
+ }
+ else rc.noId ();
+ }
+ asserv_.reset ();
+}
+
+// Destructeur.
+Motor::~Motor (void)
+{
+ instance_ = 0;
+ delete movement_;
+}
+
+// Appelée lors d'une mise à jour des compteurs.
+void
+Motor::updateCounter (int l, int r)
+{
+ double dL = l * s_;
+ double dR = r * s_;
+ tracker_.update (dL, dR);
+ if (movement_)
+ if (!movement_->control ())
+ {
+ delete movement_;
+ movement_ = 0;
+ asserv_.stop ();
+ }
+}
+
+// Choisi le mouvement en cours.
+void
+Motor::setMovement (Movement *m)
+{
+ delete movement_;
+ movement_ = m;
+ m->init (tracker_, asserv_, *this);
+}
+
+// Teste si les mouvements sont terminés.
+bool
+Motor::end (void)
+{
+ ok ();
+ return !movement_;
+}
+
+// Attend la fin de tout les mouvements.
+void
+Motor::waitEnd (void)
+{
+ do {
+ waitOk ();
+ } while (!end ());
+}
+
+// Teste si les moteurs sont arrétés.
+bool
+Motor::stopped (void)
+{
+ ok ();
+ return tracker_.stopped ();
+}
+
+// Attend que les moteurs soient arrétés.
+void
+Motor::waitStopped (void)
+{
+ do {
+ waitOk ();
+ } while (!stopped ());
+}
+
+// Paramètre la vitesse des moteurs.
+void
+Motor::speed (double l, double r)
+{
+ int vL = (int) (l / vs_);
+ int vR = (int) (r / vs_);
+ // Teste le dépassement de vitesse minimale.
+ while ((l != 0.0 || r != 0.0) && vL > -minSpeed_ && vL < minSpeed_ && vR >
+ -minSpeed_ && vR < minSpeed_)
+ {
+ l *= 1.5;
+ r *= 1.5;
+ vL = (int) (l / vs_);
+ vR = (int) (r / vs_);
+ }
+ // Teste le dépassement de vitesse maximal.
+ if (vL > maxSpeed_)
+ {
+ vL = maxSpeed_;
+ vR = (int) (r / l * vL);
+ }
+ else if (vL < -maxSpeed_)
+ {
+ vL = -maxSpeed_;
+ vR = (int) (r / l * vL);
+ }
+ if (vR > maxSpeed_)
+ {
+ vR = maxSpeed_;
+ vL = (int) (l / r * vR);
+ }
+ else if (vR < -maxSpeed_)
+ {
+ vR = -maxSpeed_;
+ vL = (int) (l / r * vR);
+ }
+ // Envois la commande.
+std::cout << "motor speed " << l << ' ' << r << ' ' << vL << ' ' << vR << std::endl;;
+ asserv_.speed (vL, vR);
+}
+
+// Va vers un point, renvois faux si atteind.
+bool
+Motor::goTo (double x, double y, double eps/*5.0*/)
+{
+ return false;
+}
+
+// Va vers un point, en formant un arc de cercle, renvois faux si atteind.
+bool
+Motor::goToArc (double x, double y, double eps/*5.0*/)
+{
+ double l, r;
+ if (tracker_.computeArcs (x, y, l, r, eps))
+ {
+std::cout << "goto arc " << l << ' ' << r;
+ if (l > 0.0)
+ l = sqrt (2.0 * asserv_.getAccel () * as_ * l);
+ else
+ l = -sqrt (2.0 * asserv_.getAccel () * as_ * -l);
+ if (r > 0.0)
+ r = sqrt (2.0 * asserv_.getAccel () * as_ * r);
+ else
+ r = -sqrt (2.0 * asserv_.getAccel () * as_ * -r);
+std::cout << ' ' << l << ' ' << r << std::endl;;
+ speed (l, r);
+ return true;
+ }
+ else
+ {
+ asserv_.stop ();
+ return false;
+ }
+}
+
diff --git a/2004/i/nono/src/motor/motor.h b/2004/i/nono/src/motor/motor.h
new file mode 100644
index 0000000..ddc32ee
--- /dev/null
+++ b/2004/i/nono/src/motor/motor.h
@@ -0,0 +1,80 @@
+#ifndef motor_h
+#define motor_h
+// motor.h
+// nono - programme du robot 2004. {{{
+//
+// Copyright (C) 2003 Nicolas Schodet
+//
+// Robot APB Team/Efrei 2004.
+// Web: http://assos.efrei.fr/robot/
+// Email: robot AT efrei DOT fr
+//
+// This program is free software; you can redistribute it and/or modify
+// it under the terms of the GNU General Public License as published by
+// the Free Software Foundation; either version 2 of the License, or
+// (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program; if not, write to the Free Software
+// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+//
+// }}}
+#include "asserv.h"
+#include "tracker.h"
+#include "movement.h"
+
+class Motor : public AsservTracker
+{
+ // Object d'asservissement.
+ Asserv asserv_;
+ // Suiveur de position.
+ Tracker tracker_;
+ // Pointeur vers l'instance unique.
+ static Motor *instance_;
+ // Echelles (mm/unit, mm/s.unit, mm/s^2.unit).
+ double s_, vs_, as_;
+ // Mouvement en cours.
+ Movement *movement_;
+ // Vitesses maximale et minimale (unit).
+ int maxSpeed_, minSpeed_;
+ public:
+ // Constructeur.
+ Motor (void);
+ // Destructeur.
+ virtual ~Motor (void);
+ // Récupère l'instance unique.
+ static Motor &getInstance (void) { return *instance_; }
+ // Appelée lors d'une mise à jour des compteurs.
+ void updateCounter (int l, int r);
+ // Choisi le mouvement en cours.
+ void setMovement (Movement *m);
+ // Active l'asservissement.
+ void go (bool fl = true) { asserv_.go (fl); }
+ // Teste si l'émission est terminée.
+ bool ok (void) { return asserv_.ok (); }
+ // Attend que toute les émissions soit terminées.
+ void waitOk (void) { asserv_.waitOk (); }
+ // Teste si les mouvements sont terminés.
+ bool end (void);
+ // Attend la fin de tout les mouvements.
+ void waitEnd (void);
+ // Teste si les moteurs sont arrétés.
+ bool stopped (void);
+ // Attend que les moteurs soient arrétés.
+ void waitStopped (void);
+ // Récupère l'object tracker.
+ const Tracker &getTracker (void) const { return tracker_; }
+ // Paramètre la vitesse des moteurs.
+ void speed (double l, double r);
+ // Va vers un point, renvois faux si atteind.
+ bool goTo (double x, double y, double eps = 5.0);
+ // Va vers un point, en formant un arc de cercle, renvois faux si atteind.
+ bool goToArc (double x, double y, double eps = 5.0);
+};
+
+#endif // motor_h
diff --git a/2004/i/nono/src/motor/movement.h b/2004/i/nono/src/motor/movement.h
new file mode 100644
index 0000000..5a3deaf
--- /dev/null
+++ b/2004/i/nono/src/motor/movement.h
@@ -0,0 +1,51 @@
+#ifndef movement_h
+#define movement_h
+// movement.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 <iostream>
+
+class Tracker;
+class Asserv;
+class Motor;
+
+class Movement
+{
+ protected:
+ const Tracker *t_;
+ Asserv *a_;
+ Motor *m_;
+ public:
+ // Destructeur.
+ virtual ~Movement (void) { }
+ // 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é.
+ virtual bool control (void) { return false; }
+};
+
+#endif // movement_h
diff --git a/2004/i/nono/src/motor/movement_basic.cc b/2004/i/nono/src/motor/movement_basic.cc
new file mode 100644
index 0000000..2b1cb6e
--- /dev/null
+++ b/2004/i/nono/src/motor/movement_basic.cc
@@ -0,0 +1,61 @@
+// movement_basic.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_basic.h"
+#include "tracker.h"
+#include "asserv.h"
+#include "motor.h"
+
+// Constructeur.
+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.
+void
+MovementBasic::init (const Tracker &t, Asserv &a, Motor &m)
+{
+ Movement::init (t, a, m);
+ sX_ = t.getX ();
+ sY_ = t.getY ();
+}
+
+// Controlle la vitesse, retourne faux si mouvement terminé.
+bool
+MovementBasic::control (void)
+{
+ double dX = t_->getX () - sX_;
+ double dY = t_->getY () - sY_;
+ if (dX * dX + dY * dY >= lenght2_)
+ {
+ a_->stop ();
+ return false;
+ }
+ m_->speed (speed_, speed_);
+ return true;
+}
+
diff --git a/2004/i/nono/src/motor/movement_basic.h b/2004/i/nono/src/motor/movement_basic.h
new file mode 100644
index 0000000..4c9a899
--- /dev/null
+++ b/2004/i/nono/src/motor/movement_basic.h
@@ -0,0 +1,47 @@
+#ifndef movement_basic_h
+#define movement_basic_h
+// movement_basic.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"
+
+class MovementBasic : public Movement
+{
+ // Vitesse.
+ double speed_;
+ // Distance au carré.
+ double lenght2_;
+ // Position de départ.
+ double sX_, sY_;
+ public:
+ // Constructeur.
+ MovementBasic (double speed, double lenght);
+ // 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é.
+ bool control (void);
+};
+
+#endif // movement_basic_h
diff --git a/2004/i/nono/src/motor/movement_goto.cc b/2004/i/nono/src/motor/movement_goto.cc
new file mode 100644
index 0000000..69c1b91
--- /dev/null
+++ b/2004/i/nono/src/motor/movement_goto.cc
@@ -0,0 +1,68 @@
+// movement_goto.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_goto.h"
+#include "tracker.h"
+#include "motor.h"
+
+// Constructeur.
+MovementGoTo::MovementGoTo (double x, double y, double eps, double dist,
+ double kpl, double kpa, double kil, double kia,
+ double kdl, double kda)
+ : dX_ (x), dY_ (y), eps_ (eps), dist_ (dist),
+ kpl_ (kpl), kpa_ (kpa), kil_ (kil), kia_ (kia), kdl_ (kdl), kda_ (kda),
+ il_ (0.0), ia_ (0.0), lel_ (0.0), lea_ (0.0)
+{
+}
+
+// Controlle la vitesse, retourne faux si mouvement terminé.
+bool
+MovementGoTo::control (void)
+{
+ // Calcule l'erreur.
+ double el, ea;
+ if (!t_->computeError (dX_, dY_, el, ea, eps_, dist_))
+ {
+ return false;
+ }
+ // Pas de marche arrière.
+ if (el < 0.0) el = 0.0;
+std::cout << "movement goto error " << el << ' ' << ea << std::endl;
+ // Calcule les intégrales saturées.
+ il_ += el;
+ if (il_ > 1000) il_ = 1000;
+ else if (il_ < -1000) il_ = -1000;
+ ia_ += ea;
+ if (ia_ > 1000) ia_ = 1000;
+ else if (ia_ < -1000) ia_ = -1000;
+ // Commande les moteurs.
+ double l = kpl_ * (el + kil_ * il_ + kdl_ * (el - lel_));
+ double a = kpa_ * (ea + kia_ * ia_ + kda_ * (ea - lea_));
+ m_->speed (l - a, l + a);
+ // Retiens l'erreur pour la dérivée.
+ lel_ = el;
+ lea_ = ea;
+ return true;
+}
+
diff --git a/2004/i/nono/src/motor/movement_goto.h b/2004/i/nono/src/motor/movement_goto.h
new file mode 100644
index 0000000..dbd4e67
--- /dev/null
+++ b/2004/i/nono/src/motor/movement_goto.h
@@ -0,0 +1,55 @@
+#ifndef movement_goto_h
+#define movement_goto_h
+// movement_goto.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"
+
+class MovementGoTo : public Movement
+{
+ // Position d'arrivée.
+ double dX_, dY_;
+ // Epsilon.
+ double eps_;
+ // Distance du point cible courant.
+ double dist_;
+ // Coefficient proportionnel linéaire et angulaire.
+ double kpl_, kpa_;
+ // Coefficient intégrals linéaire et angulaire.
+ double kil_, kia_;
+ // Coefficient de dérivée linéaire et angulaire.
+ double kdl_, kda_;
+ // Integrales.
+ double il_, ia_;
+ // Dernière valeurs d'erreur.
+ double lel_, lea_;
+ public:
+ // Constructeur.
+ MovementGoTo (double x, double y, double eps, double dist, double kpl,
+ double kpa, double kil, double kia, double kdl, double kda);
+ // Controlle la vitesse, retourne faux si mouvement terminé.
+ bool control (void);
+};
+
+#endif // movement_goto_h
diff --git a/2004/i/nono/src/motor/movement_types.h b/2004/i/nono/src/motor/movement_types.h
new file mode 100644
index 0000000..4872d89
--- /dev/null
+++ b/2004/i/nono/src/motor/movement_types.h
@@ -0,0 +1,30 @@
+#ifndef movement_types_h
+#define movement_types_h
+// movement_types.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_basic.h"
+#include "movement_goto.h"
+
+#endif // movement_types_h
diff --git a/2004/i/nono/src/motor/test_asserv.cc b/2004/i/nono/src/motor/test_asserv.cc
new file mode 100644
index 0000000..7ac096c
--- /dev/null
+++ b/2004/i/nono/src/motor/test_asserv.cc
@@ -0,0 +1,113 @@
+// test_asserv.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 "asserv.h"
+#include "erreur/erreur.h"
+#include "date/date.h"
+
+#include <iostream>
+#include <unistd.h>
+#include <stdlib.h>
+
+using namespace std;
+
+class AsservTrackerC : public AsservTracker
+{
+ int s;
+ public:
+ // Appelée lors d'une mise à jour des compteurs.
+ virtual void updateCounter (int l, int r) { s += l; }
+ // Affiche.
+ void aff (void)
+ { std::cout << s << std::endl; }
+ // Reset.
+ void reset (void)
+ { s = 0; }
+};
+
+int
+main (int argc, char **argv)
+{
+ try
+ {
+ AsservTrackerC at;
+ Asserv m (at);
+ int i = 1;
+ while (i < argc)
+ {
+ switch (argv[i][0])
+ {
+ case 's':
+ at.reset ();
+ cout << "stop" << endl;
+ i++;
+ m.stop ();
+ break;
+ case 'z':
+ cout << "rezet" << endl;
+ i++;
+ m.reset ();
+ break;
+ case 'g':
+ cout << "go" << endl;
+ i++;
+ m.go ();
+ break;
+ case 'G':
+ cout << "ungo" << endl;
+ i++;
+ m.go (false);
+ break;
+ case 'v':
+ i++;
+ int vg, vd;
+ if (i >= argc) break;
+ vg = atoi (argv[i++]);
+ if (i >= argc) break;
+ vd = atoi (argv[i++]);
+ cout << "speed " << vg << ' ' << vd << endl;
+ m.speed (vg, vd);
+ break;
+ default:
+ int s;
+ s = atoi (argv[i++]);
+ cout << "sleep " << s << endl;
+ for (int j = 0; j < s * 1000 / 50; ++j)
+ {
+ m.ok ();
+ Date::wait (1);
+ }
+ break;
+ }
+ m.waitOk ();
+ }
+ at.aff ();
+ return 0;
+ }
+ catch (Erreur &e)
+ {
+ cerr << e.what () << endl;
+ return 1;
+ }
+}
diff --git a/2004/i/nono/src/motor/test_motor.cc b/2004/i/nono/src/motor/test_motor.cc
new file mode 100644
index 0000000..325975e
--- /dev/null
+++ b/2004/i/nono/src/motor/test_motor.cc
@@ -0,0 +1,113 @@
+// test_motor.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.h"
+#include "erreur/erreur.h"
+#include "date/date.h"
+#include "movement_types.h"
+
+#include <iostream>
+#include <unistd.h>
+#include <stdlib.h>
+#include <math.h>
+
+using namespace std;
+
+int
+main (int argc, char **argv)
+{
+ try
+ {
+ Motor m;
+ int i = 1;
+ while (i < argc)
+ {
+ switch (argv[i][0])
+ {
+ case 'g':
+ cout << "go" << endl;
+ i++;
+ m.go ();
+ break;
+ case 'G':
+ cout << "ungo" << endl;
+ i++;
+ m.go (false);
+ break;
+ case 'w':
+ cout << "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 << "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 << "goto " << dX << ' ' << dY << endl;
+ Movement *mov = new MovementGoTo (dX, dY, 10.0, 0.0,
+ 1.0, 1.5,
+ 0.1, 0.1,
+ 0.7, 0.7);
+ m.setMovement (mov);
+ }
+ break;
+ default:
+ int s;
+ s = atoi (argv[i++]);
+ cout << "sleep " << s << endl;
+ for (int j = 0; j < s * 1000 / 50; ++j)
+ {
+ m.ok ();
+ Date::wait (50);
+ }
+ break;
+ }
+ m.waitEnd ();
+ }
+ return 0;
+ }
+ catch (Erreur &e)
+ {
+ cerr << e.what () << endl;
+ return 1;
+ }
+}
diff --git a/2004/i/nono/src/motor/test_tracker.cc b/2004/i/nono/src/motor/test_tracker.cc
new file mode 100644
index 0000000..197156e
--- /dev/null
+++ b/2004/i/nono/src/motor/test_tracker.cc
@@ -0,0 +1,82 @@
+// test_tracker.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 "tracker.h"
+#include "erreur/erreur.h"
+
+#include <stdlib.h>
+#include <iostream>
+
+int
+main (int argc, char **argv)
+{
+ try
+ {
+ Tracker t;
+ if (argc < 2) return 1;
+ switch (argv[1][0])
+ {
+ case 'e':
+ // Teste le calcul d'erreur.
+ for (int i = 2; i < argc - 1; i += 2)
+ {
+ double x = atof (argv[i]);
+ double y = atof (argv[i + 1]);
+ double el, ea;
+ t.computeError (x, y, el, ea, 50.0, 10.0);
+ std::cout << "error " << x << ' ' << y << ' ' << el << ' ' <<
+ ea << std::endl;
+ }
+ break;
+ case 'a':
+ // Teste le calcul d'arcs.
+ for (int i = 2; i < argc - 1; i += 2)
+ {
+ double x = atof (argv[i]);
+ double y = atof (argv[i + 1]);
+ double l, r;
+ t.computeArcs (x, y, l, r, 50.0);
+ std::cout << "arc " << x << ' ' << y << ' ' << l << ' ' << r
+ << std::endl;
+ }
+ break;
+ default:
+ // Teste les updates.
+ for (int i = 1; i < argc - 1; i += 2)
+ {
+ double l = atof (argv[i]);
+ double r = atof (argv[i + 1]);
+ t.update (l, r);
+ std::cout << "update " << l << ' ' << r << " pos " << t <<
+ std::endl;
+ }
+ }
+ return 0;
+ }
+ catch (Erreur &e)
+ {
+ cerr << e.what () << endl;
+ return 1;
+ }
+}
diff --git a/2004/i/nono/src/motor/tracker.cc b/2004/i/nono/src/motor/tracker.cc
new file mode 100644
index 0000000..4229864
--- /dev/null
+++ b/2004/i/nono/src/motor/tracker.cc
@@ -0,0 +1,230 @@
+// tracker.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 "tracker.h"
+#include "config/config.h"
+
+#include <iostream>
+#include <math.h>
+
+// Constructeur.
+Tracker::Tracker (void)
+ : posX_ (0.0), posY_ (0.0), angle_ (0.0),
+ f_ (10.0), zero_ (0)
+{
+ // Lit la conf.
+ Config rc ("rc/tracker");
+ while (!rc.eof ())
+ {
+ if (rc.isId ("startx"))
+ {
+ rc.getId ();
+ rc >> posX_;
+ }
+ else if (rc.isId ("starty"))
+ {
+ rc.getId ();
+ rc >> posY_;
+ }
+ else if (rc.isId ("startangle"))
+ {
+ rc.getId ();
+ rc >> angle_;
+ }
+ else if (rc.isId ("footing"))
+ {
+ rc.getId ();
+ rc >> f_;
+ }
+ else rc.noId ();
+ }
+}
+
+// Destructeur.
+Tracker::~Tracker (void)
+{
+}
+
+// Set the position.
+void
+Tracker::setPos (double x, double y, double angle)
+{
+ posX_ = x;
+ posY_ = y;
+ angle_ = angle;
+}
+
+// Get the position.
+void
+Tracker::getPos (double &x, double &y, double &angle) const
+{
+ x = posX_;
+ y = posY_;
+ angle = angle_;
+}
+
+// Calcule l'erreur linéaire et angulaire pour atteindre un point,
+// retourne faux si atteind.
+bool
+Tracker::computeError (double x, double y, double &el, double &ea, double eps,
+ double dist) const
+{
+ // Calcule le point cible courant.
+ double aX = posX_ + dist * cos (angle_);
+ double aY = posY_ + dist * sin (angle_);
+ // Calcule la diférence avec la cible voulue.
+ double eX = x - aX;
+ double eY = y - aY;
+ // Vérifie l'eps.
+ if (eX * eX + eY * eY < eps * eps)
+ {
+ el = 0;
+ ea = 0;
+ return false;
+ }
+ // 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
+ // perpendiculaire).
+ ea = eX * -sin (angle_) + eY * cos (angle_);
+ 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.
+bool
+Tracker::computeArcs (double x, double y, double &l, double &r, double eps)
+ const
+{
+ // Vecteur vers le point à atteindre.
+ double dX = x - posX_, dY = y - posY_;
+ // Calcule la distance au point (x, y).
+ double d = sqrt (dX * dX + dY * dY);
+ if (d < eps)
+ {
+ l = r = 0.0;
+std::cout << "compute arc eps" << std::endl;
+ return false;
+ }
+ // Calcule l'angle entre la direction courante et la direction du point à
+ // atteindre, évite d'utiliser un atan.
+ // Calcule le vecteur perpendiculaire au vecteur normal selon la
+ // direction courante.
+ double vX = -sin (angle_);
+ double vY = cos (angle_);
+ // Calcul de l'angle devant/deriere.
+ if (dX * vY + dY * -vX < 0.0)
+ {
+ l = r = 0;
+std::cout << "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;
+ // 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;
+ }
+ // 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;
+ return false;
+ }
+ else
+ {
+ // Calcule le rayon de courbure à suivre.
+ double ro = 0.5 * d / sin (dA); // sin (dA) = cos (pi/2 - dA)
+ // Rayon plus petit que l'empatement/2, stop.
+ if (ro < 0.25 * f_ && ro > 0.25 * f_)
+ {
+ l = r = 0.0;
+std::cout << "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;
+ }
+ return true;
+}
+
+// Met à jour la position.
+void
+Tracker::update (double dL, double dR)
+{
+ // Compte les zeros.
+ if (dL == 0.0 && dR == 0.0)
+ {
+ zero_++;
+ return;
+ }
+ else
+ {
+ zero_ = 0;
+ }
+ // Calcule l'angle et l'avancement moyen.
+ // 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;
+ // Si l'angle est petit, évite une division par presque 0.
+ if (dA < 0.0001 && dA > -0.0001)
+ {
+ // Considère que l'on avance en ligne droite selon la moitié de
+ // l'angle.
+ double a = angle_ + 0.5 * dA;
+ posX_ += dS * cos (a);
+ posY_ += dS * sin (a);
+ angle_ += dA;
+//std::cout << "tracker small dX " << dS * cos (a) << " dY " << dS * sin (a) << std::endl;
+ }
+ else
+ {
+ // dS : arc de cercle parcouru (grâce à Thales).
+ // R : rayon, dS = dA * R, R = dS / dA
+ // On a plus qu'à calculer les dX, dY avec cos et sin.
+ double oldA = angle_;
+ 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;
+ }
+std::cout << "update pos " << *this << std::endl;
+}
+
+// Affiche la position.
+std::ostream &
+operator<< (std::ostream &os, const Tracker &t)
+{
+ os << t.getX () << ' ' << t.getY () << ' ' << t.getAngle ();
+ return os;
+}
+
diff --git a/2004/i/nono/src/motor/tracker.h b/2004/i/nono/src/motor/tracker.h
new file mode 100644
index 0000000..5271c08
--- /dev/null
+++ b/2004/i/nono/src/motor/tracker.h
@@ -0,0 +1,66 @@
+#ifndef tracker_h
+#define tracker_h
+// tracker.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 <iostream>
+
+class Tracker
+{
+ // Position et angle (mm et rad).
+ double posX_, posY_, angle_;
+ // Distance entre les deux roues (mm).
+ double f_;
+ // Nombre d'updates nuls consécutifs.
+ int zero_;
+ public:
+ // Constructeur.
+ Tracker (void);
+ // Destructeur.
+ ~Tracker (void);
+ // Set the position.
+ void setPos (double x, double y, double angle);
+ // Get the position.
+ void getPos (double &x, double &y, double &angle) const;
+ double getX (void) const { return posX_; }
+ double getY (void) const { return posY_; }
+ double getAngle (void) const { return angle_; }
+ bool stopped (void) { return zero_ > 10; }
+ // Calcule l'erreur linéaire et angulaire pour atteindre un point,
+ // retourne faux si atteind.
+ bool computeError (double x, double y, double &el, double &ea, double eps,
+ double dist) const;
+ // Calcule la longueur de l'arc gauche et droite pour atteindre un point.
+ // EPS précise la distance minimale. Renvois faux si atteind.
+ bool computeArcs (double x, double y, double &l, double &r, double eps)
+ const;
+ // Met à jour la position.
+ void update (double dL, double dR);
+};
+
+// Affiche la position.
+std::ostream &operator<< (std::ostream &os, const Tracker &t);
+
+#endif // tracker_h