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.cc31
1 files changed, 21 insertions, 10 deletions
diff --git a/2004/i/nono/src/motor/movement_goto.cc b/2004/i/nono/src/motor/movement_goto.cc
index 6c5b198..cc720a9 100644
--- a/2004/i/nono/src/motor/movement_goto.cc
+++ b/2004/i/nono/src/motor/movement_goto.cc
@@ -68,24 +68,34 @@ MovementGoto::~MovementGoto (void)
delete goto_;
}
+/// Initialise le mouvement, appelé juste quand le mouvement est mis en
+/// service.
+void
+MovementGoto::init (const Tracker &t, Asserv &a, Motor &m)
+{
+ Movement::init (t, a, m);
+ goto_->init (t);
+}
+
/// Controlle la vitesse, retourne faux si mouvement terminé.
bool
MovementGoto::control (void)
{
- // Récupère la distance.
- double dist;
- if (!goto_->getDistance (*t_, dist, param_.eps_))
+ // Récupère la distance et le prochain point.
+ double dist, dx, dy;
+ if (!goto_->get (*t_, param_.dist_, param_.eps_, dist, dx, dy))
return false;
- // Calcule la vitesse en ligne droite.
- double speed = m_->computeSpeed (dist);
- // Récupère le prochain point.
- double dx, dy;
- goto_->getPoint (*t_, dx, dy, param_.dist_);
+std::cout << "movement goto: consign " << dist << ' ' << dx << ' ' << dy <<
+ std::endl;
// Calcule l'erreur.
double el, ea;
t_->computeError (dx, dy, el, ea);
// Pas de marche arrière.
- if (el < 0.0) el = 0.0;
+ if (el < 0.0)
+ {
+ el = 0.0;
+ ea = 2.0 - ea;
+ }
std::cout << "movement goto: error " << el << ' ' << ea << std::endl;
// Calcule les intégrales saturées.
il_ += el;
@@ -99,7 +109,8 @@ 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 (speed * (l - a), speed * (l + a));
+std::cout << "movement goto: command " << l << ' ' << a << std::endl;
+ m_->speed (l, a, dist);
// Retiens l'erreur pour la dérivée.
lel_ = el;
lea_ = ea;