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.cc49
1 files changed, 47 insertions, 2 deletions
diff --git a/2004/i/nono/src/motor/asserv.cc b/2004/i/nono/src/motor/asserv.cc
index c806479..7be14f7 100644
--- a/2004/i/nono/src/motor/asserv.cc
+++ b/2004/i/nono/src/motor/asserv.cc
@@ -30,12 +30,14 @@
/// Constructeur.
Asserv::Asserv (AsservTracker &asservTracker)
: ttyspeed_ (0), accel_ (-1), kp_ (-1), ki_ (-1), kd_ (-1),
- statMotor_ (-1), counter_ (false), posAsserv_ (false), asserv_ (false),
+ statMotor_ (-1), counter_ (false), posAsserv_ (false), gpiDelay_ (0),
+ asserv_ (false),
zeroEps_ (0),
noSetParam_ (false),
inBufSize_ (64), inBufPos_ (0), inBuf_ (0),
firstCounter_ (true),
countLeft_ (0), countRight_ (0),
+ gpi_ (0),
asservTracker_ (asservTracker),
log_ ("asserv")
{
@@ -53,13 +55,14 @@ Asserv::Asserv (AsservTracker &asservTracker)
rc.get ("stats", statMotor_) ||
rc.get ("counter", counter_) ||
rc.get ("posasserv", posAsserv_) ||
+ rc.get ("gpi", gpiDelay_) ||
rc.get ("zeroeps", zeroEps_) ||
rc.get ("buffer", inBufSize_)
))
rc.noId ();
}
// Ouvre le port série.
- serial_.open (ttyname_.c_str (), ttyspeed_);
+ serial_.open (ttyname_, ttyspeed_);
// Alloue la mémoire du tampon d'entrée.
inBuf_ = new char[inBufSize_ + 2];
}
@@ -105,6 +108,8 @@ Asserv::setParam (void)
setStatMotor (statMotor_);
if (posAsserv_)
setPosAsserv (posAsserv_);
+ if (gpiDelay_)
+ setGpiDelay (gpiDelay_);
if (asserv_)
setAsserv (asserv_);
if (counter_)
@@ -217,6 +222,13 @@ Asserv::setPosAsserv (bool fl/*true*/)
}
void
+Asserv::setGpiDelay (int delay)
+{
+ send ('h', delay);
+ gpiDelay_ = delay;
+}
+
+void
Asserv::setAsserv (bool fl/*true*/)
{
send ('g', fl);
@@ -237,6 +249,13 @@ Asserv::getAccelFactor (void) const
return getSpeedFactor () / 0.004 / accel_;
}
+/// Envois la sortie GPO.
+void
+Asserv::sendGpo (unsigned int gpo)
+{
+ send ('k', static_cast<int> (gpo));
+}
+
/// Envoie un message.
void
Asserv::send (char com)
@@ -335,6 +354,10 @@ Asserv::handleMessage (void)
// Recois une nouvelle valeur pour les compteurs.
handleCounter ();
break;
+ case 'H':
+ // Recois un GPI.
+ handleGpi ();
+ break;
case 'T':
// TTL expiré, log un message.
log_ (Log::warning) << "ttl expired" << std::endl;
@@ -449,3 +472,25 @@ Asserv::handleCounter (void)
countLeft_ = l;
countRight_ = r;
}
+
+/// Traite les GPI.
+void
+Asserv::handleGpi (void)
+{
+ if (inBufPos_ != 2 + 2 + 1)
+ {
+ // Mauvaise transmission.
+ log_ (Log::warning) << "gpi error (n)" << std::endl;
+ return;
+ }
+ unsigned int gpi = hexUnsignedChar2int (inBuf_ + 2);
+ if (gpi == static_cast<unsigned int> (hexInvalid))
+ {
+ // Mauvaise transmission.
+ log_ (Log::warning) << "gpi error (?)" << std::endl;
+ return;
+ }
+ // Recopie la valeur.
+ gpi_ = gpi;
+}
+