summaryrefslogtreecommitdiff
path: root/2004/i/nono/src/motor/movement_goto.cc
diff options
context:
space:
mode:
Diffstat (limited to '2004/i/nono/src/motor/movement_goto.cc')
-rw-r--r--2004/i/nono/src/motor/movement_goto.cc19
1 files changed, 11 insertions, 8 deletions
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;