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.cc74
1 files changed, 41 insertions, 33 deletions
diff --git a/2004/i/nono/src/motor/asserv.cc b/2004/i/nono/src/motor/asserv.cc
index 40c5944..bb259c3 100644
--- a/2004/i/nono/src/motor/asserv.cc
+++ b/2004/i/nono/src/motor/asserv.cc
@@ -25,15 +25,13 @@
#include "asserv.h"
#include "config/config.h"
#include "date/date.h"
+#include "utils/hexa.h"
#include <iostream>
-// Table de conversion en hexa.
-const char *Asserv::hexaTbl_ = "0123456789abcdef";
-
-// Constructeur.
+/// Constructeur.
Asserv::Asserv (AsservTracker &asservTracker)
- : accel_ (-1), kp_ (-1), ki_ (-1), kd_ (-1),
+ : ttyspeed_ (0), accel_ (-1), kp_ (-1), ki_ (-1), kd_ (-1),
statMotor_ (false), counter_ (false),
inBufSize_ (64), inBufPos_ (0), inBuf_ (0),
firstCounter_ (true),
@@ -50,6 +48,7 @@ Asserv::Asserv (AsservTracker &asservTracker)
rc.get ("ki", ki_) ||
rc.get ("kd", kd_) ||
rc.get ("tty", ttyname_) ||
+ rc.get ("ttyspeed", ttyspeed_) ||
rc.get ("stats", statMotor_) ||
rc.get ("counter", counter_) ||
rc.get ("buffer", inBufSize_)
@@ -62,7 +61,7 @@ Asserv::Asserv (AsservTracker &asservTracker)
inBuf_ = new char[inBufSize_];
}
-// Destructeur.
+/// Destructeur.
Asserv::~Asserv (void)
{
// Envois le signal de reset.
@@ -71,7 +70,7 @@ Asserv::~Asserv (void)
delete inBuf_;
}
-// Reset la carte et envois les paramètres.
+/// Reset la carte et envois les paramètres.
void
Asserv::reset (void)
{
@@ -96,28 +95,28 @@ Asserv::reset (void)
setCounter (counter_);
}
-// Active l'asservissement.
+/// Active l'asservissement.
void
Asserv::go (bool fl)
{
send ('g', fl);
}
-// Stop !
+/// Stop !
void
Asserv::stop (void)
{
send ('s');
}
-// Réglage de la vitesse.
+/// Réglage de la vitesse.
void
Asserv::speed (int l, int r)
{
send ('v', l, r);
}
-// Teste si l'émission est terminée.
+/// Teste si l'émission est terminée.
bool
Asserv::ok (void)
{
@@ -125,14 +124,14 @@ Asserv::ok (void)
return sendQueue_.empty ();
}
-// Attend que toute les émissions soit terminées.
+/// Attend que toute les émissions soit terminées.
void
Asserv::waitOk (void)
{
while (!ok ()) Date::wait (1);
}
-// Lit et traite les messages de la cartes.
+/// Lit et traite les messages de la cartes.
void
Asserv::read (void)
{
@@ -154,7 +153,7 @@ Asserv::read (void)
}
}
-// Change les paramètres de la carte.
+/// Change les paramètres de la carte.
void
Asserv::setAccel (int accel)
{
@@ -198,7 +197,7 @@ Asserv::setCounter (bool fl = true)
counter_ = fl;
}
-// Envoie un message.
+/// Envoie un message.
void
Asserv::send (char com)
{
@@ -231,8 +230,8 @@ Asserv::send (char com, int a1)
std::string s;
s += '!';
s += com;
- s += hexaTbl_[(a1 >> 4) & 0x0f];
- s += hexaTbl_[a1 & 0x0f];
+ s += hex2digit ((a1 >> 4) & 0x0f);
+ s += hex2digit (a1 & 0x0f);
s += '\r';
sendQueue_.push (s);
if (wasEmpty) sendLast ();
@@ -245,16 +244,16 @@ Asserv::send (char com, int a1, int a2)
std::string s;
s += '!';
s += com;
- s += hexaTbl_[(a1 >> 4) & 0x0f];
- s += hexaTbl_[a1 & 0x0f];
- s += hexaTbl_[(a2 >> 4) & 0x0f];
- s += hexaTbl_[a2 & 0x0f];
+ s += hex2digit ((a1 >> 4) & 0x0f);
+ s += hex2digit (a1 & 0x0f);
+ s += hex2digit ((a2 >> 4) & 0x0f);
+ s += hex2digit (a2 & 0x0f);
s += '\r';
sendQueue_.push (s);
if (wasEmpty) sendLast ();
}
-// Renvois le dernier message.
+/// Renvois le dernier message.
void
Asserv::sendLast (void)
{
@@ -264,7 +263,7 @@ std::cout << "send " << s << std::endl;
serial_.write (s.data (), s.size ());
}
-// Traite un message.
+/// Traite un message.
void
Asserv::handleMessage (void)
{
@@ -287,7 +286,7 @@ inBuf_[inBufPos_] = 0;
break;
case 'm':
// Recois des nouvelles stats.
- // handleStatMotor ();
+ handleStatMotor ();
break;
case 'c':
// Recois une nouvelle valeur pour les compteurs.
@@ -299,7 +298,23 @@ inBuf_[inBufPos_] = 0;
inBufPos_ = 0;
}
-// Traite un message du compteur.
+/// Traite un message de stats.
+void
+Asserv::handleStatMotor (void)
+{
+ if (inBufPos_ != 2 + 1 + 2 + 1 + 4 + 1 + 4)
+ {
+std::cout << "stat motor error" << std::endl;
+ // Mauvaise transmission.
+ return;
+ }
+ char side = inBuf_[2];
+ //int vacc = getSignedChar (inBuf_ + 2 + 1);
+ int e = getSignedShort (inBuf_ + 2 + 1 + 2 + 1);
+ int pwm = getSignedShort (inBuf_ + 2 + 1 + 2 + 1 + 4 + 1);
+}
+
+/// Traite un message du compteur.
void
Asserv::handleCounter (void)
{
@@ -337,7 +352,7 @@ std::cout << "counter error" << std::endl;
countRight_ = r;
}
-// Décode un mot signé (2 octets).
+/// Décode un mot signé (2 octets).
int
Asserv::getSignedShort (const char *s) const
{
@@ -346,10 +361,3 @@ Asserv::getSignedShort (const char *s) const
| hex2digit (s[2]) << 4
| hex2digit (s[3]));
}
-
-// Décode un chiffre hexa.
-int
-Asserv::hex2digit (char c) const
-{
- return (c >= '0' && c <= '9') ? c - '0' : c - 'a' + 10;
-}