// 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 "utils/hexa.h" /// Constructeur. Asserv::Asserv (AsservTracker &asservTracker) : ttyspeed_ (0), 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), log_ ("asserv") { // Lit la conf. Config rc ("rc/asserv"); while (!rc.eof ()) { if (!( rc.get ("accel", accel_) || rc.get ("kp", kp_) || rc.get ("ki", ki_) || rc.get ("kd", kd_) || rc.get ("tty", ttyname_) || rc.get ("ttyspeed", ttyspeed_) || rc.get ("stats", statMotor_) || rc.get ("counter", counter_) || rc.get ("buffer", inBufSize_) )) 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_ + 1]; } /// 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 += digit2hex ((a1 >> 4) & 0x0f); s += digit2hex (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 += digit2hex ((a1 >> 4) & 0x0f); s += digit2hex (a1 & 0x0f); s += digit2hex ((a2 >> 4) & 0x0f); s += digit2hex (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 (); log_ (Log::debug) << "send " << s << std::endl; serial_.write (s.data (), s.size ()); } /// Traite un message. void Asserv::handleMessage (void) { inBuf_[inBufPos_] = 0; log_ (Log::debug) << "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 de stats. void Asserv::handleStatMotor (void) { if (inBufPos_ != 2 + 1 + 2 + 1 + 4 + 1 + 4) { // Mauvaise transmission. log_ (Log::warning) << "stat motor error" << std::endl; return; } char side = inBuf_[2]; int vacc = hexSignedChar2int (inBuf_ + 2 + 1); int e = hexSignedShort2int (inBuf_ + 2 + 1 + 2 + 1); int pwm = hexSignedShort2int (inBuf_ + 2 + 1 + 2 + 1 + 4 + 1); log_ (Log::verydebug) << "stat motor " << side << ' ' << vacc << ' ' << e << ' ' << pwm << std::endl; } /// Traite un message du compteur. void Asserv::handleCounter (void) { if (inBufPos_ != 2 + 4 + 1 + 4) { // Mauvaise transmission. log_ (Log::warning) << "counter error" << std::endl; return; } int l = hexSignedShort2int (inBuf_ + 2); int r = hexSignedShort2int (inBuf_ + 2 + 4 + 1); if (firstCounter_) { // Première valeur ignorée. countLeft_ = l; countRight_ = r; firstCounter_ = false; } // Attention à l'overflow. log_ (Log::verydebug) << "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; log_ (Log::verydebug) << "counter after " << countLeft_ << ' ' << countRight_ << ' ' << l << ' ' << r << std::endl; // Met à jour le tracker. asservTracker_.updateCounter (l - countLeft_, r - countRight_); log_ (Log::verydebug) << "tracker update " << l - countLeft_ << ' ' << r - countRight_ << std::endl; // Retiens les anciennes valeurs. countLeft_ = l; countRight_ = r; }