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.cc33
1 files changed, 24 insertions, 9 deletions
diff --git a/2004/i/nono/src/motor/asserv.cc b/2004/i/nono/src/motor/asserv.cc
index 0b26c18..b50837f 100644
--- a/2004/i/nono/src/motor/asserv.cc
+++ b/2004/i/nono/src/motor/asserv.cc
@@ -30,14 +30,16 @@
/// Constructeur.
Asserv::Asserv (AsservTracker &asservTracker)
: ttyspeed_ (0), accel_ (-1), kp_ (-1), ki_ (-1), kd_ (-1),
- statMotor_ (-1), counter_ (false), posAsserv_ (false), gpiDelay_ (0),
+ 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),
- gpi_ (0),
+ countLeftSum_ (0), countRightSum_ (0),
+ gpi_ (0), gpo_ (0),
asservTracker_ (asservTracker),
log_ ("asserv")
{
@@ -96,6 +98,8 @@ Asserv::setParam (void)
inBufPos_ = 0;
while (!sendQueue_.empty ()) sendQueue_.pop ();
// Renvois les paramètres.
+ if (gpo_ != 0)
+ sendGpo (gpo_);
if (accel_ != -1)
setAccel (accel_);
if (kp_ != -1)
@@ -150,7 +154,13 @@ void
Asserv::read (void)
{
int c;
- while ((c = serial_.getchar ()) != -1)
+ c = serial_.getchar ();
+ if (c == -1 && !sendQueue_.empty () && lastSent_ + recvTimeout_ <
+ Date::getInstance ().start ())
+ {
+ sendLast ();
+ }
+ while (c != -1)
{
if (c == '\n' || c == '\r')
{
@@ -167,6 +177,7 @@ Asserv::read (void)
inBufPos_ = 0;
inBuf_[inBufPos_++] = c;
}
+ c = serial_.getchar ();
}
}
@@ -207,11 +218,11 @@ Asserv::setStatMotor (int delay)
}
void
-Asserv::setCounter (bool fl/*true*/)
+Asserv::setCounter (int delay)
{
firstCounter_ = true;
- send ('c', fl);
- counter_ = fl;
+ send ('c', delay);
+ counter_ = delay;
}
void
@@ -254,6 +265,7 @@ void
Asserv::sendGpo (unsigned int gpo)
{
send ('k', static_cast<int> (gpo));
+ gpo_ = gpo;
}
/// Envoie un message.
@@ -328,6 +340,7 @@ Asserv::sendLast (void)
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.
@@ -467,9 +480,11 @@ Asserv::handleCounter (void)
int rdiff = r - countRight_;
bool zero = rdiff > -zeroEps_ && rdiff < zeroEps_ && ldiff > -zeroEps_ &&
ldiff < zeroEps_;
- asservTracker_.updateCounter (ldiff, rdiff, zero);
- log_ (Log::verydebug) << "tracker update " << ldiff << ' ' << rdiff <<
- std::endl;
+ 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;