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.cc48
1 files changed, 40 insertions, 8 deletions
diff --git a/2004/i/nono/src/motor/asserv.cc b/2004/i/nono/src/motor/asserv.cc
index 8a2a579..c806479 100644
--- a/2004/i/nono/src/motor/asserv.cc
+++ b/2004/i/nono/src/motor/asserv.cc
@@ -31,6 +31,7 @@
Asserv::Asserv (AsservTracker &asservTracker)
: ttyspeed_ (0), accel_ (-1), kp_ (-1), ki_ (-1), kd_ (-1),
statMotor_ (-1), counter_ (false), posAsserv_ (false), asserv_ (false),
+ zeroEps_ (0),
noSetParam_ (false),
inBufSize_ (64), inBufPos_ (0), inBuf_ (0),
firstCounter_ (true),
@@ -52,6 +53,7 @@ Asserv::Asserv (AsservTracker &asservTracker)
rc.get ("stats", statMotor_) ||
rc.get ("counter", counter_) ||
rc.get ("posasserv", posAsserv_) ||
+ rc.get ("zeroeps", zeroEps_) ||
rc.get ("buffer", inBufSize_)
))
rc.noId ();
@@ -98,7 +100,7 @@ Asserv::setParam (void)
if (ki_ != -1)
setKi (ki_);
if (kd_ != -1)
- setKi (kd_);
+ setKd (kd_);
if (statMotor_ != -1)
setStatMotor (statMotor_);
if (posAsserv_)
@@ -135,7 +137,7 @@ Asserv::ok (void)
void
Asserv::waitOk (void)
{
- while (!ok ()) serial_.wait (100);
+ while (!ok ()) serial_.wait (50);
}
/// Lit et traite les messages de la cartes.
@@ -221,6 +223,20 @@ Asserv::setAsserv (bool fl/*true*/)
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_;
+}
+
/// Envoie un message.
void
Asserv::send (char com)
@@ -366,14 +382,20 @@ Asserv::handleStatMotor (void)
if (inBufPos_ != 2 + 1 + 2 + 1 + 4 + 1 + 4 + 1)
{
// Mauvaise transmission.
- log_ (Log::warning) << "stat motor error" << std::endl;
+ 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);
- log_ (Log::verydebug) << "stat motor " << side << ' ' << vacc << ' ' << e
+ 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;
}
@@ -384,11 +406,17 @@ Asserv::handleCounter (void)
if (inBufPos_ != 2 + 4 + 1 + 4 + 1)
{
// Mauvaise transmission.
- log_ (Log::warning) << "counter error" << std::endl;
+ 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.
@@ -410,9 +438,13 @@ Asserv::handleCounter (void)
log_ (Log::verydebug) << "counter after " << countLeft_ << ' ' <<
countRight_ << ' ' << l << ' ' << r << std::endl;
// Met à jour le tracker.
- asservTracker_.updateCounter (l - countLeft_, r - countRight_);
- log_ (Log::verydebug) << "tracker update " << l - countLeft_ << ' ' << r -
- countRight_ << std::endl;
+ int ldiff = l - countLeft_;
+ 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;
// Retiens les anciennes valeurs.
countLeft_ = l;
countRight_ = r;