summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorschodet2004-04-22 22:01:08 +0000
committerschodet2004-04-22 22:01:08 +0000
commitdddf37ebeac3b89c7e1cfe644ceefa85d2d02aec (patch)
tree2db2b8ee9a290004fc60655cd431f456e99f2889
parent96ee48cd9845c16b999c3e6d2caecdb448a47194 (diff)
Nouvelle version de la carte d'asservissement.
Nouvelle version MovementGoto.
-rw-r--r--2004/i/nono/src/motor/asserv.cc58
-rw-r--r--2004/i/nono/src/motor/asserv.h12
-rw-r--r--2004/i/nono/src/motor/goto.h9
-rw-r--r--2004/i/nono/src/motor/goto_simple.cc13
-rw-r--r--2004/i/nono/src/motor/goto_simple.h7
-rw-r--r--2004/i/nono/src/motor/motor.cc25
-rw-r--r--2004/i/nono/src/motor/motor.h11
-rw-r--r--2004/i/nono/src/motor/movement_goto.cc19
-rw-r--r--2004/i/nono/src/motor/test_asserv.cc4
-rw-r--r--2004/i/nono/src/motor/test_tracker.cc2
-rw-r--r--2004/i/nono/src/motor/tracker.cc40
-rw-r--r--2004/i/nono/src/motor/tracker.h21
12 files changed, 136 insertions, 85 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 ())
{
diff --git a/2004/i/nono/src/motor/asserv.h b/2004/i/nono/src/motor/asserv.h
index 2ff8c30..60b9893 100644
--- a/2004/i/nono/src/motor/asserv.h
+++ b/2004/i/nono/src/motor/asserv.h
@@ -48,8 +48,11 @@ class Asserv
int ttyspeed_;
// Paramètres.
int accel_, kp_, ki_, kd_;
- bool statMotor_, counter_;
+ int statMotor_;
+ bool counter_;
bool posAsserv_;
+ bool asserv_;
+ bool noSetParam_;
// File d'emmission.
std::queue<std::string> sendQueue_;
// Buffer de reception.
@@ -69,8 +72,8 @@ class Asserv
~Asserv (void);
/// Reset la carte et envois les paramètres.
void reset (void);
- /// Active l'asservissement.
- void go (bool fl = true);
+ /// Paramètre la carte d'asservissement.
+ void setParam (void);
/// Stop !
void stop (void);
/// Réglage de la vitesse.
@@ -88,9 +91,10 @@ class Asserv
void setKp (int kp);
void setKi (int ki);
void setKd (int kd);
- void setStatMotor (bool fl = true);
+ void setStatMotor (int delay);
void setCounter (bool fl = true);
void setPosAsserv (bool fl = true);
+ void setAsserv (bool fl = true);
/// @}
protected:
/// @{
diff --git a/2004/i/nono/src/motor/goto.h b/2004/i/nono/src/motor/goto.h
index 7dad57a..e062c5d 100644
--- a/2004/i/nono/src/motor/goto.h
+++ b/2004/i/nono/src/motor/goto.h
@@ -32,9 +32,14 @@ class Goto
public:
/// Destructeur.
virtual ~Goto (void) { }
+ /// Fournit la distance au point final (mm) et renvois false si le Goto
+ /// est fini.
+ /// eps: distance (mm) en dessous de laquelle on considère que la
+ /// destination est atteinte.
+ virtual bool getDistance (const Tracker &t, double &dist, double eps) = 0;
/// Fournit le point le plus loin à moins de dist (mm).
- virtual void get (const Tracker &t, double &dx, double &dy, double dist) =
- 0;
+ virtual void getPoint (const Tracker &t, double &dx, double &dy,
+ double dist) = 0;
};
#endif // goto_h
diff --git a/2004/i/nono/src/motor/goto_simple.cc b/2004/i/nono/src/motor/goto_simple.cc
index 1514800..fd6a16e 100644
--- a/2004/i/nono/src/motor/goto_simple.cc
+++ b/2004/i/nono/src/motor/goto_simple.cc
@@ -31,9 +31,20 @@ GotoSimple::GotoSimple (double dx, double dy)
{
}
+/// Fournit la distance au point final (mm) et renvois false si le Goto est
+/// fini.
+/// eps: distance (mm) en dessous de laquelle on considère que la
+/// destination est atteinte.
+bool
+GotoSimple::getDistance (const Tracker &t, double &dist, double eps)
+{
+ dist = t.getDistance (dx_, dy_);
+ return dist > eps;
+}
+
/// Fournit le point le plus loin à moins de dist (mm).
void
-GotoSimple::get (const Tracker &t, double &dx, double &dy, double dist)
+GotoSimple::getPoint (const Tracker &t, double &dx, double &dy, double dist)
{
dx = dx_;
dy = dy_;
diff --git a/2004/i/nono/src/motor/goto_simple.h b/2004/i/nono/src/motor/goto_simple.h
index 7401c22..950203e 100644
--- a/2004/i/nono/src/motor/goto_simple.h
+++ b/2004/i/nono/src/motor/goto_simple.h
@@ -34,8 +34,13 @@ class GotoSimple : public Goto
/// Constructeur.
/// (dx, dy) : position d'arrivée (mm).
GotoSimple (double dx, double dy);
+ /// Fournit la distance au point final (mm) et renvois false si le Goto
+ /// est fini.
+ /// eps: distance (mm) en dessous de laquelle on considère que la
+ /// destination est atteinte.
+ bool getDistance (const Tracker &t, double &dist, double eps);
/// Fournit le point le plus loin à moins de dist (mm).
- void get (const Tracker &t, double &dx, double &dy, double dist);
+ void getPoint (const Tracker &t, double &dx, double &dy, double dist);
};
#endif // goto_simple_h
diff --git a/2004/i/nono/src/motor/motor.cc b/2004/i/nono/src/motor/motor.cc
index 81b801a..7bbae46 100644
--- a/2004/i/nono/src/motor/motor.cc
+++ b/2004/i/nono/src/motor/motor.cc
@@ -34,6 +34,7 @@ Motor *Motor::instance_ = 0;
Motor::Motor (void)
: asserv_ (*this), log_ ("motor"),
s_ (1.0), vs_ (1.0), as_ (1.0),
+ brakeDistance_ (0.0),
movement_ (0), maxSpeed_ (0x40), minSpeed_ (0x04)
{
// Mémorise le pointeur d'instance.
@@ -46,13 +47,13 @@ Motor::Motor (void)
rc.get ("scale", s_) ||
rc.get ("speedscale", vs_) ||
rc.get ("accelscale", as_) ||
+ rc.get ("brakedistance", brakeDistance_) ||
rc.get ("maxspeed", maxSpeed_) ||
rc.get ("minspeed", minSpeed_)
))
rc.noId ();
}
asserv_.reset ();
- asserv_.setPosAsserv ();
}
/// Destructeur.
@@ -62,7 +63,7 @@ Motor::~Motor (void)
delete movement_;
}
-/// Appelée lors d'une mise à jour des compteurs.
+/// Appelée lors d'une mise à jour des compteurs (unit).
void
Motor::updateCounter (int l, int r)
{
@@ -125,16 +126,16 @@ Motor::waitStopped (void)
void
Motor::speed (double l, double r)
{
- int vL = (int) ((vs_ > 0 ? l : -l) * maxSpeed_);
- int vR = (int) ((vs_ > 0 ? r : -r) * maxSpeed_);
+ int vL = (int) ((vs_ > 0 ? l : -l) * maxSpeed_ * 0.5);
+ int vR = (int) ((vs_ > 0 ? r : -r) * maxSpeed_ * 0.5);
// Teste le dépassement de vitesse minimale.
while ((l != 0.0 || r != 0.0) && vL > -minSpeed_ && vL < minSpeed_ && vR >
-minSpeed_ && vR < minSpeed_)
{
l *= 1.5;
r *= 1.5;
- vL = (int) ((vs_ > 0 ? l : -l) * maxSpeed_);
- vR = (int) ((vs_ > 0 ? r : -r) * maxSpeed_);
+ vL = (int) ((vs_ > 0 ? l : -l) * maxSpeed_ * 0.5);
+ vR = (int) ((vs_ > 0 ? r : -r) * maxSpeed_ * 0.5);
}
// Teste le dépassement de vitesse maximal.
if (vL > maxSpeed_)
@@ -163,11 +164,15 @@ Motor::speed (double l, double r)
asserv_.speed (vL, vR);
}
-/// Va vers un point, renvois faux si atteind.
-bool
-Motor::goTo (double x, double y, double eps/*5.0*/)
+/// Calcule la vitesse en ligne droit afin de pouvoir freiner au bout
+/// d'une distance (mm).
+double
+Motor::computeSpeed (double dist) const
{
- return false;
+ if (dist > brakeDistance_)
+ return 1.0;
+ else
+ return dist / brakeDistance_;
}
/// Va vers un point, en formant un arc de cercle, renvois faux si atteind.
diff --git a/2004/i/nono/src/motor/motor.h b/2004/i/nono/src/motor/motor.h
index fca17c4..cd596a7 100644
--- a/2004/i/nono/src/motor/motor.h
+++ b/2004/i/nono/src/motor/motor.h
@@ -43,6 +43,8 @@ class Motor : public AsservTracker
static Motor *instance_;
/// Echelles (mm/unit, mm/s.unit, mm/s^2.unit).
double s_, vs_, as_;
+ /// Distance de freinage à vitesse maximale.
+ double brakeDistance_;
/// Mouvement en cours.
Movement *movement_;
/// Vitesses maximale et minimale (unit).
@@ -54,12 +56,12 @@ class Motor : public AsservTracker
virtual ~Motor (void);
/// Récupère l'instance unique.
static Motor &getInstance (void) { return *instance_; }
- /// Appelée lors d'une mise à jour des compteurs.
+ /// Appelée lors d'une mise à jour des compteurs (unit).
void updateCounter (int l, int r);
/// Choisi le mouvement en cours.
void setMovement (Movement *m);
/// Active l'asservissement.
- void go (bool fl = true) { asserv_.go (fl); }
+ void go (bool fl = true) { asserv_.setAsserv (fl); }
/// Teste si l'émission est terminée.
bool ok (void) { return asserv_.ok (); }
/// Attend que toute les émissions soit terminées.
@@ -76,8 +78,9 @@ class Motor : public AsservTracker
const Tracker &getTracker (void) const { return tracker_; }
/// Paramètre la vitesse des moteurs (-1..+1).
void speed (double l, double r);
- /// Va vers un point, renvois faux si atteind.
- bool goTo (double x, double y, double eps = 5.0);
+ /// Calcule la vitesse en ligne droit afin de pouvoir freiner au bout
+ /// d'une distance (mm).
+ double computeSpeed (double dist) const;
/// Va vers un point, en formant un arc de cercle, renvois faux si atteind.
/// \deprecated Utiliser goTo().
bool goToArc (double x, double y, double eps = 5.0);
diff --git a/2004/i/nono/src/motor/movement_goto.cc b/2004/i/nono/src/motor/movement_goto.cc
index e83b8ec..6c5b198 100644
--- a/2004/i/nono/src/motor/movement_goto.cc
+++ b/2004/i/nono/src/motor/movement_goto.cc
@@ -72,18 +72,21 @@ MovementGoto::~MovementGoto (void)
bool
MovementGoto::control (void)
{
- double dx, dy;
+ // Récupère la distance.
+ double dist;
+ if (!goto_->getDistance (*t_, dist, param_.eps_))
+ return false;
+ // Calcule la vitesse en ligne droite.
+ double speed = m_->computeSpeed (dist);
// Récupère le prochain point.
- goto_->get (*t_, dx, dy, param_.dist_);
+ double dx, dy;
+ goto_->getPoint (*t_, dx, dy, param_.dist_);
// Calcule l'erreur.
double el, ea;
- if (!t_->computeError (dx, dy, el, ea, param_.eps_, param_.dist_))
- {
- return false;
- }
+ t_->computeError (dx, dy, el, ea);
// Pas de marche arrière.
if (el < 0.0) el = 0.0;
-std::cout << "movement goto error " << el << ' ' << ea << std::endl;
+std::cout << "movement goto: error " << el << ' ' << ea << std::endl;
// Calcule les intégrales saturées.
il_ += el;
if (il_ > param_.is_) il_ = param_.is_;
@@ -96,7 +99,7 @@ std::cout << "movement goto error " << el << ' ' << ea << std::endl;
+ param_.kdl_ * (el - lel_));
double a = param_.kpa_ * (ea + param_.kia_ * ia_
+ param_.kda_ * (ea - lea_));
- m_->speed (l - a, l + a);
+ m_->speed (speed * (l - a), speed * (l + a));
// Retiens l'erreur pour la dérivée.
lel_ = el;
lea_ = ea;
diff --git a/2004/i/nono/src/motor/test_asserv.cc b/2004/i/nono/src/motor/test_asserv.cc
index 4aa05ae..cd8b55e 100644
--- a/2004/i/nono/src/motor/test_asserv.cc
+++ b/2004/i/nono/src/motor/test_asserv.cc
@@ -89,12 +89,12 @@ main (int argc, char **argv)
case 'g':
cout << "go" << endl;
i++;
- m.go ();
+ m.setAsserv ();
break;
case 'G':
cout << "ungo" << endl;
i++;
- m.go (false);
+ m.setAsserv (false);
break;
case 'v':
i++;
diff --git a/2004/i/nono/src/motor/test_tracker.cc b/2004/i/nono/src/motor/test_tracker.cc
index 197156e..8dd448d 100644
--- a/2004/i/nono/src/motor/test_tracker.cc
+++ b/2004/i/nono/src/motor/test_tracker.cc
@@ -44,7 +44,7 @@ main (int argc, char **argv)
double x = atof (argv[i]);
double y = atof (argv[i + 1]);
double el, ea;
- t.computeError (x, y, el, ea, 50.0, 10.0);
+ t.computeError (x, y, el, ea);
std::cout << "error " << x << ' ' << y << ' ' << el << ' ' <<
ea << std::endl;
}
diff --git a/2004/i/nono/src/motor/tracker.cc b/2004/i/nono/src/motor/tracker.cc
index e5779da..4ec28b9 100644
--- a/2004/i/nono/src/motor/tracker.cc
+++ b/2004/i/nono/src/motor/tracker.cc
@@ -81,44 +81,28 @@ Tracker::getAngleDiff (double a) const
return diff;
}
-/// Calcule l'erreur linéaire et angulaire pour atteindre un point,
-/// retourne faux si atteind.
+/// Calcule l'erreur linéaire et angulaire pour atteindre un point.
/// (x, y) : point à atteindre (mm).
/// el, ea : erreur linéaire et angulaire (-1..+1).
-/// eps : distance en dessous de laquelle on considère qu'on est arrivé (mm).
-/// dist : Distance maximale du point de référence (mm).
-bool
-Tracker::computeError (double x, double y, double &el, double &ea, double eps,
- double dist) const
+void
+Tracker::computeError (double x, double y, double &el, double &ea) const
{
// Calcule la diférence avec la cible voulue.
double eX = x - posX_;
double eY = y - posY_;
- // Calcule la distance au carré.
- double d = eX * eX + eY * eY;
- // Vérifie l'eps.
- if (d < eps * eps)
- {
- el = 0;
- ea = 0;
- return false;
- }
- // Vérifie la distance max.
- if (d > dist * dist)
- {
- double ds = sqrt (d);
- eX = eX / ds * dist;
- eY = eY / ds * dist;
- }
+ // Calcule la distance.
+ double d = sqrt (eX * eX + eY * eY);
// Ramène sur -1..+1
- eX /= dist;
- eY /= dist;
+ eX /= d;
+ eY /= d;
+ // Calcule sin et cos.
+ double sa = sin (angle_);
+ double ca = cos (angle_);
// Calcule l'erreur linéaire (projection sur la direction courante).
- el = eX * cos (angle_) + eY * sin (angle_);
+ el = eX * ca + eY * sa;
// Calcule l'erreur angulaire (projection sur la direction
// perpendiculaire).
- ea = eX * -sin (angle_) + eY * cos (angle_);
- return true;
+ ea = eX * -sa + eY * ca;
}
/// Calcule l'erreur angulaire pour atteindre un angle, retourne faux si
diff --git a/2004/i/nono/src/motor/tracker.h b/2004/i/nono/src/motor/tracker.h
index 63c58b2..9648e3e 100644
--- a/2004/i/nono/src/motor/tracker.h
+++ b/2004/i/nono/src/motor/tracker.h
@@ -27,6 +27,7 @@
#include "logger/log.h"
#include <iostream>
+#include <cmath>
/// Classe de suivi des coordonnée et de calcul d'erreur.
class Tracker
@@ -54,19 +55,16 @@ class Tracker
double getY (void) const { return posY_; }
/// Get angle.
double getAngle (void) const { return angle_; }
+ /// Get distance.
+ double getDistance (double x, double y) const;
/// Retourne vrai si arrété.
bool stopped (void) { return zero_ > 10; }
/// Récupère l'angle entre l'angle courant et a normalisé entre -pi et pi.
double getAngleDiff (double a) const;
- /// Calcule l'erreur linéaire et angulaire pour atteindre un point,
- /// retourne faux si atteind.
+ /// Calcule l'erreur linéaire et angulaire pour atteindre un point.
/// (x, y) : point à atteindre (mm).
/// el, ea : erreur linéaire et angulaire (-1..+1).
- /// eps : distance en dessous de laquelle on considère qu'on est arrivé
- /// (mm).
- /// dist : Distance maximale du point de référence (mm).
- bool computeError (double x, double y, double &el, double &ea, double eps,
- double dist) const;
+ void computeError (double x, double y, double &el, double &ea) const;
/// Calcule l'erreur angulaire pour atteindre un angle, retourne faux si
/// atteind.
/// a : angle à atteindre (rad).
@@ -85,4 +83,13 @@ class Tracker
/// Affiche la position.
std::ostream &operator<< (std::ostream &os, const Tracker &t);
+/// Get distance.
+inline double
+Tracker::getDistance (double x, double y) const
+{
+ double dx = x - posX_;
+ double dy = y - posY_;
+ return sqrt (dx * dx + dy * dy);
+}
+
#endif // tracker_h