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.cc26
1 files changed, 14 insertions, 12 deletions
diff --git a/2004/i/nono/src/motor/asserv.cc b/2004/i/nono/src/motor/asserv.cc
index 3c1a9f7..2107908 100644
--- a/2004/i/nono/src/motor/asserv.cc
+++ b/2004/i/nono/src/motor/asserv.cc
@@ -27,8 +27,6 @@
#include "date/date.h"
#include "utils/hexa.h"
-#include <iostream>
-
/// Constructeur.
Asserv::Asserv (AsservTracker &asservTracker)
: ttyspeed_ (0), accel_ (-1), kp_ (-1), ki_ (-1), kd_ (-1),
@@ -36,7 +34,8 @@ Asserv::Asserv (AsservTracker &asservTracker)
inBufSize_ (64), inBufPos_ (0), inBuf_ (0),
firstCounter_ (true),
countLeft_ (0), countRight_ (0),
- asservTracker_ (asservTracker)
+ asservTracker_ (asservTracker),
+ log_ ("asserv")
{
// Lit la conf.
Config rc ("rc/asserv");
@@ -58,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_];
+ inBuf_ = new char[inBufSize_ + 1];
}
/// Destructeur.
@@ -259,7 +258,7 @@ Asserv::sendLast (void)
{
if (sendQueue_.empty ()) return;
std::string &s = sendQueue_.front ();
-std::cout << "send " << s << std::endl;
+ log_ (Log::debug) << "send " << s << std::endl;
serial_.write (s.data (), s.size ());
}
@@ -267,8 +266,8 @@ std::cout << "send " << s << std::endl;
void
Asserv::handleMessage (void)
{
-inBuf_[inBufPos_] = 0;
-//std::cout << "recv " << inBuf_ << std::endl;
+ inBuf_[inBufPos_] = 0;
+ log_ (Log::debug) << "recv " << inBuf_ << std::endl;
if (inBufPos_ > 1 && inBuf_[0] == '!')
{
switch (inBuf_[1])
@@ -304,8 +303,8 @@ Asserv::handleStatMotor (void)
{
if (inBufPos_ != 2 + 1 + 2 + 1 + 4 + 1 + 4)
{
-std::cout << "stat motor error" << std::endl;
// Mauvaise transmission.
+ log_ (Log::warning) << "stat motor error" << std::endl;
return;
}
char side = inBuf_[2];
@@ -320,8 +319,8 @@ Asserv::handleCounter (void)
{
if (inBufPos_ != 2 + 4 + 1 + 4)
{
-std::cout << "counter error" << std::endl;
// Mauvaise transmission.
+ log_ (Log::warning) << "counter error" << std::endl;
return;
}
int l = getSignedShort (inBuf_ + 2);
@@ -334,7 +333,8 @@ std::cout << "counter error" << std::endl;
firstCounter_ = false;
}
// Attention à l'overflow.
-//std::cout << "counter before " << countLeft_ << ' ' << countRight_ << ' ' << l << ' ' << r << std::endl;
+ log_ (Log::verydebug) << "counter before " << countLeft_ << ' ' <<
+ countRight_ << ' ' << l << ' ' << r << std::endl;
if (l > 0x4000 && countLeft_ < -0x4000)
countLeft_ += 0x10000;
if (l < -0x4000 && countLeft_ > 0x4000)
@@ -343,10 +343,12 @@ std::cout << "counter error" << std::endl;
countRight_ += 0x10000;
if (r < -0x4000 && countRight_ > 0x4000)
countRight_ -= 0x10000;
-//std::cout << "counter after " << countLeft_ << ' ' << countRight_ << ' ' << l << ' ' << r << std::endl;
+ log_ (Log::verydebug) << "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;
+ log_ (Log::verydebug) << "tracker update " << l - countLeft_ << ' ' << r -
+ countRight_ << std::endl;
// Retiens les anciennes valeurs.
countLeft_ = l;
countRight_ = r;