// 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_ (-1), counter_ (0), posAsserv_ (false), gpiDelay_ (0), asserv_ (false), zeroEps_ (0), noSetParam_ (false), lastSent_ (-1000), inBufSize_ (64), inBufPos_ (0), inBuf_ (0), firstCounter_ (true), countLeft_ (0), countRight_ (0), countLeftSum_ (0), countRightSum_ (0), gpi_ (0), gpo_ (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 ("posasserv", posAsserv_) || rc.get ("gpi", gpiDelay_) || rc.get ("zeroeps", zeroEps_) || rc.get ("buffer", inBufSize_) )) rc.noId (); } // Ouvre le port série. serial_.open (ttyname_, ttyspeed_); // Alloue la mémoire du tampon d'entrée. inBuf_ = new char[inBufSize_ + 2]; } /// Destructeur. Asserv::~Asserv (void) { // Envois le signal de reset. noSetParam_ = true; reset (); waitOk (); delete inBuf_; } /// Reset la carte et envois les paramètres. void Asserv::reset (void) { // Vide la file. while (!sendQueue_.empty ()) sendQueue_.pop (); // Envois le signal de reset. send ('z'); } /// Paramètre la carte d'asservissement. void Asserv::setParam (void) { // Reset les données. countLeft_ = countRight_ = 0; inBufPos_ = 0; while (!sendQueue_.empty ()) sendQueue_.pop (); // Renvois les paramètres. if (gpo_ != 0) sendGpo (gpo_); if (accel_ != -1) setAccel (accel_); if (kp_ != -1) setKp (kp_); if (ki_ != -1) setKi (ki_); if (kd_ != -1) setKd (kd_); if (statMotor_ != -1) setStatMotor (statMotor_); if (posAsserv_) setPosAsserv (posAsserv_); if (gpiDelay_) setGpiDelay (gpiDelay_); if (asserv_) setAsserv (asserv_); if (counter_) setCounter (counter_); } /// 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 ()) serial_.wait (50); } /// Lit et traite les messages de la cartes. void Asserv::read (void) { int c; c = serial_.getchar (); if (c == -1 && !sendQueue_.empty () && lastSent_ + recvTimeout_ < Date::getInstance ().start ()) { sendLast (); } while (c != -1) { if (c == '\n' || c == '\r') { if (inBufPos_) { inBuf_[inBufPos_++] = '\r'; // Tampon plein. handleMessage (); } } else { if (c == '!' || inBufPos_ >= inBufSize_) inBufPos_ = 0; inBuf_[inBufPos_++] = c; } c = serial_.getchar (); } } /// 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 (int delay) { send ('m', delay); statMotor_ = delay; } void Asserv::setCounter (int delay) { firstCounter_ = true; send ('c', delay); counter_ = delay; } void Asserv::setPosAsserv (bool fl/*true*/) { send ('V', fl); posAsserv_ = fl; } void Asserv::setGpiDelay (int delay) { send ('h', delay); gpiDelay_ = delay; } void Asserv::setAsserv (bool fl/*true*/) { send ('g', fl); asserv_ = fl; } /// Get factor to deduce speed scale from scale. double Asserv::getSpeedFactor (void) const { return 1.0 / 0.004; } /// Get factor to deduce accel scale from scale. double Asserv::getAccelFactor (void) const { return getSpeedFactor () / 0.004 / accel_; } /// Envois la sortie GPO. void Asserv::sendGpo (unsigned int gpo) { send ('k', static_cast (gpo)); gpo_ = gpo; } /// Envoie un message. void Asserv::send (char com) { std::string s; s += '!'; s += com; s += '\r'; send (s); } void Asserv::send (char com, bool fl) { std::string s; s += '!'; s += com; s += fl ? '1' : '0'; s += '\r'; send (s); } void Asserv::send (char com, int a1) { std::string s; s += '!'; s += com; s += digit2hex ((a1 >> 4) & 0x0f); s += digit2hex (a1 & 0x0f); s += '\r'; send (s); } void Asserv::send (char com, int a1, int a2) { 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'; send (s); } /// Envois le message à la carte. void Asserv::send (const std::string &m) { bool wasEmpty = ok (); // Traitement spécial pour la commande de vitesse non aquitée en // posAsserv_, non envoyé si il y a quelquechose dans la file. if (!posAsserv_ || m[1] != 'v') sendQueue_.push (m); if (wasEmpty) { log_ (Log::debug) << "send " << m << std::endl; serial_.write (m.data (), m.size ()); } } /// 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 ()); lastSent_ = Date::getInstance ().start (); } /// Traite un message. void Asserv::handleMessage (void) { inBuf_[inBufPos_] = 0; log_ (Log::debug) << "recv " << inBuf_ << std::endl; if (inBufPos_ >= 3 && inBuf_[0] == '!') { if (noSetParam_ && inBuf_[1] != 'z') inBuf_[1] = '*'; switch (inBuf_[1]) { case 'E': // Erreur, renvois la dernière commande, on est du genre // obstiné... log_ (Log::warning) << "transmission error (E)" << std::endl; sendLast (); break; case 'M': // Recois des nouvelles stats. handleStatMotor (); break; case 'C': // Recois une nouvelle valeur pour les compteurs. handleCounter (); break; case 'H': // Recois un GPI. handleGpi (); break; case 'T': // TTL expiré, log un message. log_ (Log::warning) << "ttl expired" << std::endl; break; case 'z': // Reset reçu, on reparamètre la carte. if (!noSetParam_) { log_ (Log::warning) << "rezet" << std::endl; setParam (); break; } // Sinon, laisse passer... default: if (!sendQueue_.empty ()) { if (sendQueue_.front () == inBuf_) // Ok. sendQueue_.pop (); else // Certainement une erreur. log_ (Log::warning) << "transmission error (ack mismatch) : " << inBuf_ << std::endl; } sendLast (); break; } } else { // Surement une erreur, on renvois. log_ (Log::warning) << "transmission error (n < 2) : " << inBuf_ << std::endl; sendLast (); } // Efface le tampon. inBufPos_ = 0; } /// Traite un message de stats. void Asserv::handleStatMotor (void) { if (inBufPos_ != 2 + 1 + 2 + 1 + 4 + 1 + 4 + 1) { // Mauvaise transmission. log_ (Log::warning) << "stat motor error (n)" << 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); if (vacc == hexInvalid || e == hexInvalid || pwm == hexInvalid) { // Mauvaise transmission. log_ (Log::warning) << "stat motor error (?)" << std::endl; return; } log_ (Log::debug) << "stat motor " << side << ' ' << vacc << ' ' << e << ' ' << pwm << std::endl; } /// Traite un message du compteur. void Asserv::handleCounter (void) { if (inBufPos_ != 2 + 4 + 1 + 4 + 1) { // Mauvaise transmission. log_ (Log::warning) << "counter error (n)" << std::endl; return; } int l = hexSignedShort2int (inBuf_ + 2); int r = hexSignedShort2int (inBuf_ + 2 + 4 + 1); if (l == hexInvalid || r == hexInvalid) { // Mauvaise transmission. log_ (Log::warning) << "counter error (?)" << std::endl; return; } 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. int ldiff = l - countLeft_; int rdiff = r - countRight_; bool zero = rdiff > -zeroEps_ && rdiff < zeroEps_ && ldiff > -zeroEps_ && ldiff < zeroEps_; countLeftSum_ += ldiff; countRightSum_ += rdiff; asservTracker_.updateCounter (countLeftSum_, countRightSum_, zero); log_ (Log::verydebug) << "tracker update " << countLeftSum_ << ' ' << countRightSum_ << std::endl; // Retiens les anciennes valeurs. countLeft_ = l; countRight_ = r; } /// Traite les GPI. void Asserv::handleGpi (void) { if (inBufPos_ != 2 + 2 + 1) { // Mauvaise transmission. log_ (Log::warning) << "gpi error (n)" << std::endl; return; } unsigned int gpi = hexUnsignedChar2int (inBuf_ + 2); if (gpi == static_cast (hexInvalid)) { // Mauvaise transmission. log_ (Log::warning) << "gpi error (?)" << std::endl; return; } // Recopie la valeur. gpi_ = gpi; }