summaryrefslogtreecommitdiff
path: root/2004/i/nono/src/motor/asserv.cc
diff options
context:
space:
mode:
Diffstat (limited to '2004/i/nono/src/motor/asserv.cc')
-rw-r--r--2004/i/nono/src/motor/asserv.cc385
1 files changed, 385 insertions, 0 deletions
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;
+}