summaryrefslogtreecommitdiff
path: root/2004/i/nono/src/motor/motor.cc
diff options
context:
space:
mode:
Diffstat (limited to '2004/i/nono/src/motor/motor.cc')
-rw-r--r--2004/i/nono/src/motor/motor.cc121
1 files changed, 88 insertions, 33 deletions
diff --git a/2004/i/nono/src/motor/motor.cc b/2004/i/nono/src/motor/motor.cc
index ddd00ce..902e46f 100644
--- a/2004/i/nono/src/motor/motor.cc
+++ b/2004/i/nono/src/motor/motor.cc
@@ -33,7 +33,7 @@ Motor *Motor::instance_ = 0;
/// Constructeur.
Motor::Motor (void)
: asserv_ (*this), log_ ("motor"),
- s_ (1.0), vs_ (1.0), a_ (1.0),
+ s_ (1.0), vs_ (1.0), a_ (1.0), fact_ (0.5),
brakeDistance_ (0.0),
movement_ (0), maxSpeed_ (0x40), minSpeed_ (0x04)
{
@@ -45,6 +45,7 @@ Motor::Motor (void)
{
if (!(
rc.get ("scale", s_) ||
+ rc.get ("fact", fact_) ||
rc.get ("maxspeed", maxSpeed_) ||
rc.get ("minspeed", minSpeed_)
))
@@ -63,8 +64,8 @@ Motor::Motor (void)
/// Destructeur.
Motor::~Motor (void)
{
+ clearMovement ();
instance_ = 0;
- delete movement_;
}
/// Appelée lors d'une mise à jour des compteurs.
@@ -76,22 +77,43 @@ Motor::updateCounter (int l, int r, bool zero)
double dL = l * s_;
double dR = r * s_;
tracker_.update (dL, dR, zero);
+ if (!movement_)
+ {
+ if (!nextMovement ())
+ return;
+ }
if (movement_)
- if (!movement_->control ())
+ {
+ while (!movement_->control ())
{
- delete movement_;
- movement_ = 0;
- asserv_.stop ();
+ if (!nextMovement ())
+ {
+ // C'est la fin.
+ asserv_.stop ();
+ return;
+ }
}
+ }
+
+
+}
+
+/// Ajoute un mouvement à la file d'attente.
+void
+Motor::addMovement (Movement *m)
+{
+ movementQueue_.push_back (m);
}
-/// Choisi le mouvement en cours.
+/// Vide la file d'attente.
void
-Motor::setMovement (Movement *m)
+Motor::clearMovement (void)
{
delete movement_;
- movement_ = m;
- m->init (tracker_, asserv_, *this);
+ MovementQueue::iterator first = movementQueue_.begin ();
+ MovementQueue::iterator last = movementQueue_.end ();
+ for ( ; first != last; ++first)
+ delete *first;
}
/// Teste si les mouvements sont terminés.
@@ -99,7 +121,7 @@ bool
Motor::end (void)
{
ok ();
- return !movement_;
+ return !movement_ && movementQueue_.empty ();
}
/// Attend la fin de tout les mouvements.
@@ -129,64 +151,83 @@ Motor::waitStopped (void)
}
/// Paramètre la vitesse linéaire (-1..+1) et angulaire (-1..+1). Limite
-/// la vitesse pour pouvoir freiner au bout de dist (mm).
+/// la vitesse pour pouvoir freiner au bout de dist (mm) et au bout de
+/// l'angle (rad).
void
-Motor::speed (double l, double a, double dist)
+Motor::speed (double l, double a, double dist, double angle)
{
- speedLimit (l - a, l + a, computeSpeed (dist));
+ double liml, lima;
+ liml = computeSpeed (dist);
+ lima = computeSpeed (tracker_.getAngleDistance (angle));
+ speedLimit (l - a, l + a, liml, lima);
}
/// Paramètre la vitesse des moteurs (-1..+1).
void
Motor::speed (double l, double r)
{
- speedLimit (l, r, maxSpeed_);
+ speedLimit (l, r, maxSpeed_, 2 * maxSpeed_);
}
/// Paramètre la vitesse des moteurs (-1..+1) avec une vitesse limite
-/// (unit).
+/// linéaire en angulaire (unit).
void
-Motor::speedLimit (double l, double r, double vLimit)
+Motor::speedLimit (double l, double r, double vLimitL, double vLimitA)
{
// Il faut un minimum tout de même. La politique, c'est on avance.
- if (vLimit < minSpeed_)
- vLimit = minSpeed_;
+ if (vLimitL < minSpeed_)
+ vLimitL = minSpeed_;
+ if (vLimitA < minSpeed_)
+ vLimitA = minSpeed_;
// Convertie le (-1..+1) en (unit).
- int vL = (int) ((vs_ > 0 ? l : -l) * maxSpeed_ * 0.5);
- int vR = (int) ((vs_ > 0 ? r : -r) * maxSpeed_ * 0.5);
+ int vL = (int) ((vs_ > 0 ? l : -l) * maxSpeed_ * fact_);
+ int vR = (int) ((vs_ > 0 ? r : -r) * maxSpeed_ * fact_);
// 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_ * 0.5);
- vR = (int) ((vs_ > 0 ? r : -r) * maxSpeed_ * 0.5);
+ vL = (int) ((vs_ > 0 ? l : -l) * maxSpeed_ * fact_);
+ vR = (int) ((vs_ > 0 ? r : -r) * maxSpeed_ * fact_);
}
- // Teste le dépassement de vitesse maximal.
- if (vL > vLimit)
+ // Teste le dépassement de vitesse maximale linéaire.
+ if (vL > vLimitL)
{
- vL = static_cast<int> (vLimit);
+ vL = static_cast<int> (vLimitL);
vR = (int) (r / l * vL);
}
- else if (vL < -vLimit)
+ else if (vL < -vLimitL)
{
- vL = static_cast<int> (-vLimit);
+ vL = static_cast<int> (-vLimitL);
vR = (int) (r / l * vL);
}
- if (vR > vLimit)
+ if (vR > vLimitL)
{
- vR = static_cast<int> (vLimit);
+ vR = static_cast<int> (vLimitL);
vL = (int) (l / r * vR);
}
- else if (vR < -vLimit)
+ else if (vR < -vLimitL)
{
- vR = static_cast<int> (-vLimit);
+ vR = static_cast<int> (-vLimitL);
vL = (int) (l / r * vR);
}
+ // Teste le dépassement de vitesse maximale angulaire.
+ if (vL - vR > vLimitA)
+ {
+ double fact = vLimitA / (vL - vR);
+ vR = static_cast<int> (vR * fact);
+ vL = static_cast<int> (vL * fact);
+ }
+ else if (vR - vL > vLimitA)
+ {
+ double fact = vLimitA / (vR - vL);
+ vR = static_cast<int> (vR * fact);
+ vL = static_cast<int> (vL * fact);
+ }
// Envois la commande.
log_ (Log::debug) << "speed " << l << ' ' << r << ' ' << vL << ' '
- << vR << ' ' << vLimit << std::endl;;
+ << vR << ' ' << vLimitL << ' ' << vLimitA << std::endl;;
asserv_.speed (vL, vR);
}
@@ -231,3 +272,17 @@ Motor::goToArc (double x, double y, double eps/*5.0*/)
}
}
+/// Passe au mouvement suivant.
+bool
+Motor::nextMovement (void)
+{
+ delete movement_;
+ movement_ = 0;
+ if (movementQueue_.empty ())
+ return false;
+ movement_ = movementQueue_.front ();
+ movementQueue_.pop_front ();
+ movement_->init (tracker_, asserv_, *this);
+ return true;
+}
+