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.cc96
1 files changed, 67 insertions, 29 deletions
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;