From 4b1255944d0fe58e700681b52512f1d62405d9ec Mon Sep 17 00:00:00 2001 From: schodet Date: Sat, 17 Apr 2004 17:12:33 +0000 Subject: Nouvelle version du protocol asservissement. --- 2004/i/nono/runtime/rc/movement/goto | 2 +- 2004/i/nono/src/motor/asserv.cc | 96 +++++++++++++++++++++--------- 2004/i/nono/src/motor/asserv.h | 4 ++ 2004/i/nono/src/motor/motor.cc | 1 + 2004/i/nono/src/motor/movement_goto.cc | 4 +- 2004/i/nono/src/motor/movement_rotation.cc | 2 +- 2004/i/nono/src/utils/hexa.cc | 4 +- 2004/i/nono/src/utils/hexa.h | 4 +- 8 files changed, 80 insertions(+), 37 deletions(-) (limited to '2004') diff --git a/2004/i/nono/runtime/rc/movement/goto b/2004/i/nono/runtime/rc/movement/goto index 874538a..478fee9 100644 --- a/2004/i/nono/runtime/rc/movement/goto +++ b/2004/i/nono/runtime/rc/movement/goto @@ -2,4 +2,4 @@ epsilon 100 distance 150 kpl 1.0 kil 0.1 kdl 0.7 kpa 1.0 kia 0.1 kda 0.7 -is 1000 +is 20 diff --git a/2004/i/nono/src/motor/asserv.cc b/2004/i/nono/src/motor/asserv.cc index adb3fb1..2faa62b 100644 --- a/2004/i/nono/src/motor/asserv.cc +++ b/2004/i/nono/src/motor/asserv.cc @@ -57,7 +57,7 @@ Asserv::Asserv (AsservTracker &asservTracker) // Ouvre le port série. serial_.open (ttyname_.c_str ()); // Alloue la mémoire du tampon d'entrée. - inBuf_ = new char[inBufSize_ + 1]; + inBuf_ = new char[inBufSize_ + 2]; } /// Destructeur. @@ -140,8 +140,11 @@ Asserv::read (void) if (c == '\n' || c == '\r') { if (inBufPos_) + { + inBuf_[inBufPos_++] = '\r'; // Tampon plein. handleMessage (); + } } else { @@ -182,64 +185,64 @@ Asserv::setKd (int kd) } void -Asserv::setStatMotor (bool fl) +Asserv::setStatMotor (bool fl/*true*/) { send ('m', fl); statMotor_ = fl; } void -Asserv::setCounter (bool fl = true) +Asserv::setCounter (bool fl/*true*/) { firstCounter_ = true; send ('c', fl); counter_ = fl; } +void +Asserv::setPosAsserv (bool fl/*true*/) +{ + send ('V', fl); + posAsserv_ = 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 (); + send (s); } 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 (); + send (s); } 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 (); + send (s); } void Asserv::send (char com, int a1, int a2) { - bool wasEmpty = ok (); std::string s; s += '!'; s += com; @@ -248,8 +251,23 @@ Asserv::send (char com, int a1, int a2) s += digit2hex ((a2 >> 4) & 0x0f); s += digit2hex (a2 & 0x0f); s += '\r'; - sendQueue_.push (s); - if (wasEmpty) sendLast (); + 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. @@ -268,31 +286,51 @@ Asserv::handleMessage (void) { inBuf_[inBufPos_] = 0; log_ (Log::debug) << "recv " << inBuf_ << std::endl; - if (inBufPos_ > 1 && inBuf_[0] == '!') + if (inBufPos_ >= 3 && 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. + 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': + case 'M': // Recois des nouvelles stats. handleStatMotor (); break; - case 'c': + case 'C': // Recois une nouvelle valeur pour les compteurs. handleCounter (); break; + case 'T': + // TTL expiré, log un message. + log_ (Log::warning) << "ttl expired" << std::endl; + break; + 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; } @@ -301,7 +339,7 @@ Asserv::handleMessage (void) void Asserv::handleStatMotor (void) { - if (inBufPos_ != 2 + 1 + 2 + 1 + 4 + 1 + 4) + if (inBufPos_ != 2 + 1 + 2 + 1 + 4 + 1 + 4 + 1) { // Mauvaise transmission. log_ (Log::warning) << "stat motor error" << std::endl; @@ -319,7 +357,7 @@ Asserv::handleStatMotor (void) void Asserv::handleCounter (void) { - if (inBufPos_ != 2 + 4 + 1 + 4) + if (inBufPos_ != 2 + 4 + 1 + 4 + 1) { // Mauvaise transmission. log_ (Log::warning) << "counter error" << std::endl; diff --git a/2004/i/nono/src/motor/asserv.h b/2004/i/nono/src/motor/asserv.h index 5685bff..2ff8c30 100644 --- a/2004/i/nono/src/motor/asserv.h +++ b/2004/i/nono/src/motor/asserv.h @@ -49,6 +49,7 @@ class Asserv // Paramètres. int accel_, kp_, ki_, kd_; bool statMotor_, counter_; + bool posAsserv_; // File d'emmission. std::queue sendQueue_; // Buffer de reception. @@ -89,6 +90,7 @@ class Asserv void setKd (int kd); void setStatMotor (bool fl = true); void setCounter (bool fl = true); + void setPosAsserv (bool fl = true); /// @} protected: /// @{ @@ -98,6 +100,8 @@ class Asserv void send (char com, int a1); void send (char com, int a1, int a2); /// @} + /// Envois le message à la carte. + void send (const std::string &m); /// Renvois le dernier message. void sendLast (void); /// Traite un message. diff --git a/2004/i/nono/src/motor/motor.cc b/2004/i/nono/src/motor/motor.cc index f41dda8..81b801a 100644 --- a/2004/i/nono/src/motor/motor.cc +++ b/2004/i/nono/src/motor/motor.cc @@ -52,6 +52,7 @@ Motor::Motor (void) rc.noId (); } asserv_.reset (); + asserv_.setPosAsserv (); } /// Destructeur. diff --git a/2004/i/nono/src/motor/movement_goto.cc b/2004/i/nono/src/motor/movement_goto.cc index b96526a..78ae491 100644 --- a/2004/i/nono/src/motor/movement_goto.cc +++ b/2004/i/nono/src/motor/movement_goto.cc @@ -31,8 +31,8 @@ MovementGotoParam MovementGoto::param_; /// Constructeur, charge les paramètres depuis la Config. MovementGotoParam::MovementGotoParam (void) - : eps_ (10.0), dist_ (0.0), - kpl_ (1.0), kpa_ (1.0), kil_ (0.0), kia_ (0.0), is_ (1000.0), + : eps_ (10.0), dist_ (100.0), + kpl_ (1.0), kpa_ (1.0), kil_ (0.0), kia_ (0.0), is_ (20.0), kdl_ (0.0), kda_ (0.0) { // Lit la conf. diff --git a/2004/i/nono/src/motor/movement_rotation.cc b/2004/i/nono/src/motor/movement_rotation.cc index 2f58e45..ee49f3e 100644 --- a/2004/i/nono/src/motor/movement_rotation.cc +++ b/2004/i/nono/src/motor/movement_rotation.cc @@ -32,7 +32,7 @@ MovementRotationParam MovementRotation::param_; /// Constructeur, charge les paramètres depuis la Config. MovementRotationParam::MovementRotationParam (void) : eps_ (0.01), - kpa_ (1.0), kia_ (0.0), is_ (1000.0), + kpa_ (1.0), kia_ (0.0), is_ (20.0), kda_ (0.0) { // Lit la conf. diff --git a/2004/i/nono/src/utils/hexa.cc b/2004/i/nono/src/utils/hexa.cc index ab43f64..fbcf4e9 100644 --- a/2004/i/nono/src/utils/hexa.cc +++ b/2004/i/nono/src/utils/hexa.cc @@ -64,7 +64,7 @@ digit2hex (int d) /// Décode un mot signé (1 octets). int -hexSignedChar2int (const char *s) const +hexSignedChar2int (const char *s) { return (signed char) (hex2digit (s[0]) << 4 | hex2digit (s[1]) << 0); @@ -72,7 +72,7 @@ hexSignedChar2int (const char *s) const /// Décode un mot signé (2 octets). int -hexSignedShort2int (const char *s) const +hexSignedShort2int (const char *s) { return (short) (hex2digit (s[0]) << 12 | hex2digit (s[1]) << 8 diff --git a/2004/i/nono/src/utils/hexa.h b/2004/i/nono/src/utils/hexa.h index cdae943..54ee9dc 100644 --- a/2004/i/nono/src/utils/hexa.h +++ b/2004/i/nono/src/utils/hexa.h @@ -32,9 +32,9 @@ int hex2digit (char c); char digit2hex (int d); /// Décode un mot signé (1 octets). -int hexSignedChar2int (const char *s) const; +int hexSignedChar2int (const char *s); /// Décode un mot signé (2 octets). -int hexSignedShort2int (const char *s) const; +int hexSignedShort2int (const char *s); #endif // hexa_h -- cgit v1.2.3