summaryrefslogtreecommitdiff
path: root/2004
diff options
context:
space:
mode:
authorschodet2004-04-17 17:12:33 +0000
committerschodet2004-04-17 17:12:33 +0000
commit4b1255944d0fe58e700681b52512f1d62405d9ec (patch)
treecb53584549316d61a08cd28bad4430ba20ae7311 /2004
parentbf43f96467369c8ce632a18766c04760ca233193 (diff)
Nouvelle version du protocol asservissement.
Diffstat (limited to '2004')
-rw-r--r--2004/i/nono/runtime/rc/movement/goto2
-rw-r--r--2004/i/nono/src/motor/asserv.cc96
-rw-r--r--2004/i/nono/src/motor/asserv.h4
-rw-r--r--2004/i/nono/src/motor/motor.cc1
-rw-r--r--2004/i/nono/src/motor/movement_goto.cc4
-rw-r--r--2004/i/nono/src/motor/movement_rotation.cc2
-rw-r--r--2004/i/nono/src/utils/hexa.cc4
-rw-r--r--2004/i/nono/src/utils/hexa.h4
8 files changed, 80 insertions, 37 deletions
diff --git a/2004/i/nono/runtime/rc/movement/goto b/2004/i/nono/runtime/rc/movement/goto
index 874538a..478fee9 100644
--- a/2004/i/nono/runtime/rc/movement/goto
+++ b/2004/i/nono/runtime/rc/movement/goto
@@ -2,4 +2,4 @@ epsilon 100
distance 150
kpl 1.0 kil 0.1 kdl 0.7
kpa 1.0 kia 0.1 kda 0.7
-is 1000
+is 20
diff --git a/2004/i/nono/src/motor/asserv.cc b/2004/i/nono/src/motor/asserv.cc
index adb3fb1..2faa62b 100644
--- a/2004/i/nono/src/motor/asserv.cc
+++ b/2004/i/nono/src/motor/asserv.cc
@@ -57,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_ + 1];
+ inBuf_ = new char[inBufSize_ + 2];
}
/// Destructeur.
@@ -140,8 +140,11 @@ Asserv::read (void)
if (c == '\n' || c == '\r')
{
if (inBufPos_)
+ {
+ inBuf_[inBufPos_++] = '\r';
// Tampon plein.
handleMessage ();
+ }
}
else
{
@@ -182,64 +185,64 @@ Asserv::setKd (int kd)
}
void
-Asserv::setStatMotor (bool fl)
+Asserv::setStatMotor (bool fl/*true*/)
{
send ('m', fl);
statMotor_ = fl;
}
void
-Asserv::setCounter (bool fl = true)
+Asserv::setCounter (bool fl/*true*/)
{
firstCounter_ = true;
send ('c', fl);
counter_ = fl;
}
+void
+Asserv::setPosAsserv (bool fl/*true*/)
+{
+ send ('V', fl);
+ posAsserv_ = fl;
+}
+
/// Envoie un message.
void
Asserv::send (char com)
{
- bool wasEmpty = ok ();
std::string s;
s += '!';
s += com;
s += '\r';
- sendQueue_.push (s);
- if (wasEmpty) sendLast ();
+ send (s);
}
void
Asserv::send (char com, bool fl)
{
- bool wasEmpty = ok ();
std::string s;
s += '!';
s += com;
s += fl ? '1' : '0';
s += '\r';
- sendQueue_.push (s);
- if (wasEmpty) sendLast ();
+ send (s);
}
void
Asserv::send (char com, int a1)
{
- bool wasEmpty = ok ();
std::string s;
s += '!';
s += com;
s += digit2hex ((a1 >> 4) & 0x0f);
s += digit2hex (a1 & 0x0f);
s += '\r';
- sendQueue_.push (s);
- if (wasEmpty) sendLast ();
+ send (s);
}
void
Asserv::send (char com, int a1, int a2)
{
- bool wasEmpty = ok ();
std::string s;
s += '!';
s += com;
@@ -248,8 +251,23 @@ Asserv::send (char com, int a1, int a2)
s += digit2hex ((a2 >> 4) & 0x0f);
s += digit2hex (a2 & 0x0f);
s += '\r';
- sendQueue_.push (s);
- if (wasEmpty) sendLast ();
+ send (s);
+}
+
+/// Envois le message à la carte.
+void
+Asserv::send (const std::string &m)
+{
+ bool wasEmpty = ok ();
+ // Traitement spécial pour la commande de vitesse non aquitée en
+ // posAsserv_, non envoyé si il y a quelquechose dans la file.
+ if (!posAsserv_ || m[1] != 'v')
+ sendQueue_.push (m);
+ if (wasEmpty)
+ {
+ log_ (Log::debug) << "send " << m << std::endl;
+ serial_.write (m.data (), m.size ());
+ }
}
/// Renvois le dernier message.
@@ -268,31 +286,51 @@ Asserv::handleMessage (void)
{
inBuf_[inBufPos_] = 0;
log_ (Log::debug) << "recv " << inBuf_ << std::endl;
- if (inBufPos_ > 1 && inBuf_[0] == '!')
+ if (inBufPos_ >= 3 && inBuf_[0] == '!')
{
switch (inBuf_[1])
{
- case 'o':
- case 'z':
- // Ok.
- if (!sendQueue_.empty ())
- sendQueue_.pop ();
- sendLast ();
- break;
- case 'e':
- // Erreur, renvois la dernière commande.
+ case 'E':
+ // Erreur, renvois la dernière commande, on est du genre
+ // obstiné...
+ log_ (Log::warning) << "transmission error (E)" << std::endl;
sendLast ();
break;
- case 'm':
+ case 'M':
// Recois des nouvelles stats.
handleStatMotor ();
break;
- case 'c':
+ case 'C':
// Recois une nouvelle valeur pour les compteurs.
handleCounter ();
break;
+ case 'T':
+ // TTL expiré, log un message.
+ log_ (Log::warning) << "ttl expired" << std::endl;
+ break;
+ default:
+ if (!sendQueue_.empty ())
+ {
+ if (sendQueue_.front () == inBuf_)
+ // Ok.
+ sendQueue_.pop ();
+ else
+ // Certainement une erreur.
+ log_ (Log::warning)
+ << "transmission error (ack mismatch) : "
+ << inBuf_ << std::endl;
+ }
+ sendLast ();
+ break;
}
}
+ else
+ {
+ // Surement une erreur, on renvois.
+ log_ (Log::warning) << "transmission error (n < 2) : " << inBuf_ <<
+ std::endl;
+ sendLast ();
+ }
// Efface le tampon.
inBufPos_ = 0;
}
@@ -301,7 +339,7 @@ Asserv::handleMessage (void)
void
Asserv::handleStatMotor (void)
{
- if (inBufPos_ != 2 + 1 + 2 + 1 + 4 + 1 + 4)
+ if (inBufPos_ != 2 + 1 + 2 + 1 + 4 + 1 + 4 + 1)
{
// Mauvaise transmission.
log_ (Log::warning) << "stat motor error" << std::endl;
@@ -319,7 +357,7 @@ Asserv::handleStatMotor (void)
void
Asserv::handleCounter (void)
{
- if (inBufPos_ != 2 + 4 + 1 + 4)
+ if (inBufPos_ != 2 + 4 + 1 + 4 + 1)
{
// Mauvaise transmission.
log_ (Log::warning) << "counter error" << std::endl;
diff --git a/2004/i/nono/src/motor/asserv.h b/2004/i/nono/src/motor/asserv.h
index 5685bff..2ff8c30 100644
--- a/2004/i/nono/src/motor/asserv.h
+++ b/2004/i/nono/src/motor/asserv.h
@@ -49,6 +49,7 @@ class Asserv
// Paramètres.
int accel_, kp_, ki_, kd_;
bool statMotor_, counter_;
+ bool posAsserv_;
// File d'emmission.
std::queue<std::string> sendQueue_;
// Buffer de reception.
@@ -89,6 +90,7 @@ class Asserv
void setKd (int kd);
void setStatMotor (bool fl = true);
void setCounter (bool fl = true);
+ void setPosAsserv (bool fl = true);
/// @}
protected:
/// @{
@@ -98,6 +100,8 @@ class Asserv
void send (char com, int a1);
void send (char com, int a1, int a2);
/// @}
+ /// Envois le message à la carte.
+ void send (const std::string &m);
/// Renvois le dernier message.
void sendLast (void);
/// Traite un message.
diff --git a/2004/i/nono/src/motor/motor.cc b/2004/i/nono/src/motor/motor.cc
index f41dda8..81b801a 100644
--- a/2004/i/nono/src/motor/motor.cc
+++ b/2004/i/nono/src/motor/motor.cc
@@ -52,6 +52,7 @@ Motor::Motor (void)
rc.noId ();
}
asserv_.reset ();
+ asserv_.setPosAsserv ();
}
/// Destructeur.
diff --git a/2004/i/nono/src/motor/movement_goto.cc b/2004/i/nono/src/motor/movement_goto.cc
index b96526a..78ae491 100644
--- a/2004/i/nono/src/motor/movement_goto.cc
+++ b/2004/i/nono/src/motor/movement_goto.cc
@@ -31,8 +31,8 @@ MovementGotoParam MovementGoto::param_;
/// Constructeur, charge les paramètres depuis la Config.
MovementGotoParam::MovementGotoParam (void)
- : eps_ (10.0), dist_ (0.0),
- kpl_ (1.0), kpa_ (1.0), kil_ (0.0), kia_ (0.0), is_ (1000.0),
+ : eps_ (10.0), dist_ (100.0),
+ kpl_ (1.0), kpa_ (1.0), kil_ (0.0), kia_ (0.0), is_ (20.0),
kdl_ (0.0), kda_ (0.0)
{
// Lit la conf.
diff --git a/2004/i/nono/src/motor/movement_rotation.cc b/2004/i/nono/src/motor/movement_rotation.cc
index 2f58e45..ee49f3e 100644
--- a/2004/i/nono/src/motor/movement_rotation.cc
+++ b/2004/i/nono/src/motor/movement_rotation.cc
@@ -32,7 +32,7 @@ MovementRotationParam MovementRotation::param_;
/// Constructeur, charge les paramètres depuis la Config.
MovementRotationParam::MovementRotationParam (void)
: eps_ (0.01),
- kpa_ (1.0), kia_ (0.0), is_ (1000.0),
+ kpa_ (1.0), kia_ (0.0), is_ (20.0),
kda_ (0.0)
{
// Lit la conf.
diff --git a/2004/i/nono/src/utils/hexa.cc b/2004/i/nono/src/utils/hexa.cc
index ab43f64..fbcf4e9 100644
--- a/2004/i/nono/src/utils/hexa.cc
+++ b/2004/i/nono/src/utils/hexa.cc
@@ -64,7 +64,7 @@ digit2hex (int d)
/// Décode un mot signé (1 octets).
int
-hexSignedChar2int (const char *s) const
+hexSignedChar2int (const char *s)
{
return (signed char) (hex2digit (s[0]) << 4
| hex2digit (s[1]) << 0);
@@ -72,7 +72,7 @@ hexSignedChar2int (const char *s) const
/// Décode un mot signé (2 octets).
int
-hexSignedShort2int (const char *s) const
+hexSignedShort2int (const char *s)
{
return (short) (hex2digit (s[0]) << 12
| hex2digit (s[1]) << 8
diff --git a/2004/i/nono/src/utils/hexa.h b/2004/i/nono/src/utils/hexa.h
index cdae943..54ee9dc 100644
--- a/2004/i/nono/src/utils/hexa.h
+++ b/2004/i/nono/src/utils/hexa.h
@@ -32,9 +32,9 @@ int hex2digit (char c);
char digit2hex (int d);
/// Décode un mot signé (1 octets).
-int hexSignedChar2int (const char *s) const;
+int hexSignedChar2int (const char *s);
/// Décode un mot signé (2 octets).
-int hexSignedShort2int (const char *s) const;
+int hexSignedShort2int (const char *s);
#endif // hexa_h