// 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 // 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; }