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.cc58
1 files changed, 41 insertions, 17 deletions
diff --git a/2004/i/nono/src/motor/asserv.cc b/2004/i/nono/src/motor/asserv.cc
index 2faa62b..8a2a579 100644
--- a/2004/i/nono/src/motor/asserv.cc
+++ b/2004/i/nono/src/motor/asserv.cc
@@ -30,7 +30,8 @@
/// Constructeur.
Asserv::Asserv (AsservTracker &asservTracker)
: ttyspeed_ (0), accel_ (-1), kp_ (-1), ki_ (-1), kd_ (-1),
- statMotor_ (false), counter_ (false),
+ statMotor_ (-1), counter_ (false), posAsserv_ (false), asserv_ (false),
+ noSetParam_ (false),
inBufSize_ (64), inBufPos_ (0), inBuf_ (0),
firstCounter_ (true),
countLeft_ (0), countRight_ (0),
@@ -50,12 +51,13 @@ Asserv::Asserv (AsservTracker &asservTracker)
rc.get ("ttyspeed", ttyspeed_) ||
rc.get ("stats", statMotor_) ||
rc.get ("counter", counter_) ||
+ rc.get ("posasserv", posAsserv_) ||
rc.get ("buffer", inBufSize_)
))
rc.noId ();
}
// Ouvre le port série.
- serial_.open (ttyname_.c_str ());
+ serial_.open (ttyname_.c_str (), ttyspeed_);
// Alloue la mémoire du tampon d'entrée.
inBuf_ = new char[inBufSize_ + 2];
}
@@ -64,7 +66,8 @@ Asserv::Asserv (AsservTracker &asservTracker)
Asserv::~Asserv (void)
{
// Envois le signal de reset.
- send ('z');
+ noSetParam_ = true;
+ reset ();
waitOk ();
delete inBuf_;
}
@@ -73,12 +76,20 @@ Asserv::~Asserv (void)
void
Asserv::reset (void)
{
+ // Vide la file.
+ while (!sendQueue_.empty ()) sendQueue_.pop ();
+ // Envois le signal de reset.
+ send ('z');
+}
+
+/// Paramètre la carte d'asservissement.
+void
+Asserv::setParam (void)
+{
// Reset les données.
countLeft_ = countRight_ = 0;
inBufPos_ = 0;
while (!sendQueue_.empty ()) sendQueue_.pop ();
- // Envois le signal de reset.
- send ('z');
// Renvois les paramètres.
if (accel_ != -1)
setAccel (accel_);
@@ -88,19 +99,16 @@ Asserv::reset (void)
setKi (ki_);
if (kd_ != -1)
setKi (kd_);
- if (statMotor_)
+ if (statMotor_ != -1)
setStatMotor (statMotor_);
+ if (posAsserv_)
+ setPosAsserv (posAsserv_);
+ if (asserv_)
+ setAsserv (asserv_);
if (counter_)
setCounter (counter_);
}
-/// Active l'asservissement.
-void
-Asserv::go (bool fl)
-{
- send ('g', fl);
-}
-
/// Stop !
void
Asserv::stop (void)
@@ -127,7 +135,7 @@ Asserv::ok (void)
void
Asserv::waitOk (void)
{
- while (!ok ()) Date::wait (1);
+ while (!ok ()) serial_.wait (100);
}
/// Lit et traite les messages de la cartes.
@@ -185,10 +193,10 @@ Asserv::setKd (int kd)
}
void
-Asserv::setStatMotor (bool fl/*true*/)
+Asserv::setStatMotor (int delay)
{
- send ('m', fl);
- statMotor_ = fl;
+ send ('m', delay);
+ statMotor_ = delay;
}
void
@@ -206,6 +214,13 @@ Asserv::setPosAsserv (bool fl/*true*/)
posAsserv_ = fl;
}
+void
+Asserv::setAsserv (bool fl/*true*/)
+{
+ send ('g', fl);
+ asserv_ = fl;
+}
+
/// Envoie un message.
void
Asserv::send (char com)
@@ -308,6 +323,15 @@ Asserv::handleMessage (void)
// TTL expiré, log un message.
log_ (Log::warning) << "ttl expired" << std::endl;
break;
+ case 'z':
+ // Reset reçu, on reparamètre la carte.
+ if (!noSetParam_)
+ {
+ log_ (Log::warning) << "rezet" << std::endl;
+ setParam ();
+ break;
+ }
+ // Sinon, laisse passer...
default:
if (!sendQueue_.empty ())
{