From d4adf20d86ce0e1e887523042d79e22a5930d954 Mon Sep 17 00:00:00 2001 From: schodet Date: Sun, 27 Jun 2004 14:36:36 +0000 Subject: Add: support du delay compteur. Add: support du timeout. Add: support du gpo. Mod: méthode d'intégration entière. Add: recalage. Add: round cruise. Add: limitation de la vitesse angulaire. Add: ajustement de l'angle et la position. --- 2004/i/nono/src/motor/asserv.cc | 33 +++++--- 2004/i/nono/src/motor/asserv.h | 12 ++- 2004/i/nono/src/motor/goto_tracker.cc | 16 ++-- 2004/i/nono/src/motor/motor.cc | 28 ++++++- 2004/i/nono/src/motor/motor.h | 9 +++ 2004/i/nono/src/motor/motor_cmd.cc | 13 +++ 2004/i/nono/src/motor/movement_goto.cc | 7 +- 2004/i/nono/src/motor/movement_reposition.cc | 114 +++++++++++++++++++++++++++ 2004/i/nono/src/motor/movement_reposition.h | 67 ++++++++++++++++ 2004/i/nono/src/motor/movement_rotation.cc | 2 +- 2004/i/nono/src/motor/tracker.cc | 76 ++++++++++++------ 2004/i/nono/src/motor/tracker.h | 14 +++- 12 files changed, 337 insertions(+), 54 deletions(-) create mode 100644 2004/i/nono/src/motor/movement_reposition.cc create mode 100644 2004/i/nono/src/motor/movement_reposition.h (limited to '2004') diff --git a/2004/i/nono/src/motor/asserv.cc b/2004/i/nono/src/motor/asserv.cc index 0b26c18..b50837f 100644 --- a/2004/i/nono/src/motor/asserv.cc +++ b/2004/i/nono/src/motor/asserv.cc @@ -30,14 +30,16 @@ /// Constructeur. Asserv::Asserv (AsservTracker &asservTracker) : ttyspeed_ (0), accel_ (-1), kp_ (-1), ki_ (-1), kd_ (-1), - statMotor_ (-1), counter_ (false), posAsserv_ (false), gpiDelay_ (0), + statMotor_ (-1), counter_ (0), posAsserv_ (false), gpiDelay_ (0), asserv_ (false), zeroEps_ (0), noSetParam_ (false), + lastSent_ (-1000), inBufSize_ (64), inBufPos_ (0), inBuf_ (0), firstCounter_ (true), countLeft_ (0), countRight_ (0), - gpi_ (0), + countLeftSum_ (0), countRightSum_ (0), + gpi_ (0), gpo_ (0), asservTracker_ (asservTracker), log_ ("asserv") { @@ -96,6 +98,8 @@ Asserv::setParam (void) inBufPos_ = 0; while (!sendQueue_.empty ()) sendQueue_.pop (); // Renvois les paramètres. + if (gpo_ != 0) + sendGpo (gpo_); if (accel_ != -1) setAccel (accel_); if (kp_ != -1) @@ -150,7 +154,13 @@ void Asserv::read (void) { int c; - while ((c = serial_.getchar ()) != -1) + c = serial_.getchar (); + if (c == -1 && !sendQueue_.empty () && lastSent_ + recvTimeout_ < + Date::getInstance ().start ()) + { + sendLast (); + } + while (c != -1) { if (c == '\n' || c == '\r') { @@ -167,6 +177,7 @@ Asserv::read (void) inBufPos_ = 0; inBuf_[inBufPos_++] = c; } + c = serial_.getchar (); } } @@ -207,11 +218,11 @@ Asserv::setStatMotor (int delay) } void -Asserv::setCounter (bool fl/*true*/) +Asserv::setCounter (int delay) { firstCounter_ = true; - send ('c', fl); - counter_ = fl; + send ('c', delay); + counter_ = delay; } void @@ -254,6 +265,7 @@ void Asserv::sendGpo (unsigned int gpo) { send ('k', static_cast (gpo)); + gpo_ = gpo; } /// Envoie un message. @@ -328,6 +340,7 @@ Asserv::sendLast (void) std::string &s = sendQueue_.front (); log_ (Log::debug) << "send " << s << std::endl; serial_.write (s.data (), s.size ()); + lastSent_ = Date::getInstance ().start (); } /// Traite un message. @@ -467,9 +480,11 @@ Asserv::handleCounter (void) int rdiff = r - countRight_; bool zero = rdiff > -zeroEps_ && rdiff < zeroEps_ && ldiff > -zeroEps_ && ldiff < zeroEps_; - asservTracker_.updateCounter (ldiff, rdiff, zero); - log_ (Log::verydebug) << "tracker update " << ldiff << ' ' << rdiff << - std::endl; + countLeftSum_ += ldiff; + countRightSum_ += rdiff; + asservTracker_.updateCounter (countLeftSum_, countRightSum_, zero); + log_ (Log::verydebug) << "tracker update " << countLeftSum_ << ' ' << + countRightSum_ << std::endl; // Retiens les anciennes valeurs. countLeft_ = l; countRight_ = r; diff --git a/2004/i/nono/src/motor/asserv.h b/2004/i/nono/src/motor/asserv.h index aa39324..50d2627 100644 --- a/2004/i/nono/src/motor/asserv.h +++ b/2004/i/nono/src/motor/asserv.h @@ -51,22 +51,26 @@ class Asserv // Paramètres. int accel_, kp_, ki_, kd_; int statMotor_; - bool counter_; + int counter_; bool posAsserv_; int gpiDelay_; bool asserv_; int zeroEps_; bool noSetParam_; - // File d'emmission. + // File d'emission. std::queue sendQueue_; + static const int recvTimeout_ = 500; + int lastSent_; // Buffer de reception. int inBufSize_, inBufPos_; char *inBuf_; // Anciènne valeur des compteur. bool firstCounter_; int countLeft_, countRight_; - // Valeur de l'entrée GPI. + int countLeftSum_, countRightSum_; + // Valeurs GPIO. unsigned int gpi_; + unsigned int gpo_; // Objet interessé par les stats. AsservTracker &asservTracker_; // Système de log. @@ -97,7 +101,7 @@ class Asserv void setKi (int ki); void setKd (int kd); void setStatMotor (int delay); - void setCounter (bool fl = true); + void setCounter (int delay); void setPosAsserv (bool fl = true); void setGpiDelay (int delay); void setAsserv (bool fl = true); diff --git a/2004/i/nono/src/motor/goto_tracker.cc b/2004/i/nono/src/motor/goto_tracker.cc index adc7c36..a501bc2 100644 --- a/2004/i/nono/src/motor/goto_tracker.cc +++ b/2004/i/nono/src/motor/goto_tracker.cc @@ -52,21 +52,19 @@ GotoTracker::get (const Tracker &t, double distmax, double eps, double &dist, { if (!track_) { - x_ = t.getX (); - y_ = t.getY (); + return false; } else { - x_ = t.getX () + dx_ * cos (t.getAngle ()); - y_ = t.getY () + dx_ * sin (t.getAngle ()); + double ca = cos (t.getAngle ()); + double sa = sin (t.getAngle ()); + x_ = t.getX () + dx_ * ca - dy_ * sa; + y_ = t.getY () + dx_ * sa + dy_ * ca; } changed_ = false; } - x = x_; - y = y_; - double dx = x_ - t.getX (); - double dy = y_ - t.getY (); - dist = sqrt (dx * dx + dy * dy); + dist = t.getDistance (x_, y_); + t.getPoint (x_, y_, x, y, distmax); return true; } diff --git a/2004/i/nono/src/motor/motor.cc b/2004/i/nono/src/motor/motor.cc index 902e46f..3fcb626 100644 --- a/2004/i/nono/src/motor/motor.cc +++ b/2004/i/nono/src/motor/motor.cc @@ -80,7 +80,10 @@ Motor::updateCounter (int l, int r, bool zero) if (!movement_) { if (!nextMovement ()) + { + asserv_.speed (0, 0); return; + } } if (movement_) { @@ -89,7 +92,6 @@ Motor::updateCounter (int l, int r, bool zero) if (!nextMovement ()) { // C'est la fin. - asserv_.stop (); return; } } @@ -159,7 +161,29 @@ Motor::speed (double l, double a, double dist, double angle) double liml, lima; liml = computeSpeed (dist); lima = computeSpeed (tracker_.getAngleDistance (angle)); - speedLimit (l - a, l + a, liml, lima); + speedLimit (l - a, l + a, liml, lima); +} + +/// Paramètre la vitesse linéaire (-1..+1) gauche et droite. Limite la +/// vitesse pour pouvoir freiner au bout de dist (mm) et au bout de +/// l'angle (rad). +void +Motor::speedLR (double l, double r, double dist, double angle) +{ + double liml, lima; + liml = computeSpeed (dist); + lima = computeSpeed (tracker_.getAngleDistance (angle)); + speedLimit (l, r, liml, lima); +} + +/// Paramètre la vitesse angulaire (-1..+1). Limite la vitesse pour +/// pouvoir freiner au bout de l'angle (rad). +void +Motor::rotate (double a, double angle) +{ + double lima; + lima = computeSpeed (tracker_.getAngleDistance (angle)); + speedLimit (-a, +a, maxSpeed_, lima); } /// Paramètre la vitesse des moteurs (-1..+1). diff --git a/2004/i/nono/src/motor/motor.h b/2004/i/nono/src/motor/motor.h index b33ede3..beb4cd7 100644 --- a/2004/i/nono/src/motor/motor.h +++ b/2004/i/nono/src/motor/motor.h @@ -85,6 +85,8 @@ class Motor : public AsservTracker /// Attend que les moteurs soient arrétés. void waitStopped (void); /// Récupère l'object tracker. + Tracker &getTracker (void) { return tracker_; } + /// Récupère l'object tracker. const Tracker &getTracker (void) const { return tracker_; } /// Récupère l'object asserv. Asserv &getAsserv (void) { return asserv_; } @@ -92,6 +94,13 @@ class Motor : public AsservTracker /// la vitesse pour pouvoir freiner au bout de dist (mm) et au bout de /// l'angle (rad). void speed (double l, double a, double dist, double angle); + /// Paramètre la vitesse linéaire (-1..+1) gauche et droite. Limite la + /// vitesse pour pouvoir freiner au bout de dist (mm) et au bout de + /// l'angle (rad). + void speedLR (double l, double r, double dist, double angle); + /// Paramètre la vitesse angulaire (-1..+1). Limite la vitesse pour + /// pouvoir freiner au bout de l'angle (rad). + void rotate (double a, double angle); /// Paramètre la vitesse des moteurs (-1..+1). void speed (double l, double r); /// Paramètre la vitesse des moteurs (-1..+1) avec une vitesse limite diff --git a/2004/i/nono/src/motor/motor_cmd.cc b/2004/i/nono/src/motor/motor_cmd.cc index 578ed3d..4361f59 100644 --- a/2004/i/nono/src/motor/motor_cmd.cc +++ b/2004/i/nono/src/motor/motor_cmd.cc @@ -37,6 +37,7 @@ const char motorHelp[] = " mouvement basique\n" " t va en (x, y)\n" " r se dirige vers l'angle a en degrés\n" + " R Tourne à la vitesse v\n" " c Suis un arc de rayon r et d'angle a en degrés\n" " h [p ]...\n" " trace une courbe d'hermite\n"; @@ -101,6 +102,18 @@ motorCmd (int argc, char **argv, int &i, Motor &m) m.addMovement (mov); } break; + case 'R': + { + i++; + double s; + if (i >= argc) break; + s = atof (argv[i++]); + std::cout << "test: round cruise " << s << std::endl; + Movement *mov = new MovementRoundCruise (s * 2 * M_PI / + 360); + m.addMovement (mov); + } + break; case 'c': { i++; diff --git a/2004/i/nono/src/motor/movement_goto.cc b/2004/i/nono/src/motor/movement_goto.cc index efaf959..40784a7 100644 --- a/2004/i/nono/src/motor/movement_goto.cc +++ b/2004/i/nono/src/motor/movement_goto.cc @@ -85,8 +85,7 @@ MovementGoto::control (void) double dist, dx, dy; if (!goto_->get (*t_, param_.dist_, param_.eps_, dist, dx, dy)) return false; -std::cout << "movement goto: consign " << dist << ' ' << dx << ' ' << dy << - std::endl; +//std::cout << "movement goto: consign " << dist << ' ' << dx << ' ' << dy << std::endl; // Calcule l'erreur. double el, ea, angle; t_->computeError (dx, dy, el, ea, angle); @@ -96,7 +95,7 @@ std::cout << "movement goto: consign " << dist << ' ' << dx << ' ' << dy << el = 0.0; ea = 2.0 - ea; } -std::cout << "movement goto: error " << el << ' ' << ea << ' ' << angle << std::endl; +//std::cout << "movement goto: error " << el << ' ' << ea << ' ' << angle << std::endl; // Calcule les intégrales saturées. il_ += el; if (il_ > param_.is_) il_ = param_.is_; @@ -109,7 +108,7 @@ std::cout << "movement goto: error " << el << ' ' << ea << ' ' << angle << std:: + param_.kdl_ * (el - lel_)); double a = param_.kpa_ * (ea + param_.kia_ * ia_ + param_.kda_ * (ea - lea_)); -std::cout << "movement goto: command " << l << ' ' << a << std::endl; +//std::cout << "movement goto: command " << l << ' ' << a << std::endl; m_->speed (l, a, dist, angle); // Retiens l'erreur pour la dérivée. lel_ = el; diff --git a/2004/i/nono/src/motor/movement_reposition.cc b/2004/i/nono/src/motor/movement_reposition.cc new file mode 100644 index 0000000..3ccbd86 --- /dev/null +++ b/2004/i/nono/src/motor/movement_reposition.cc @@ -0,0 +1,114 @@ +// movement_reposition.cc +// nono - programme du robot 2004. {{{ +// +// Copyright (C) 2004 Nicolas Schodet +// +// Robot APB Team/Efrei 2004. +// Web: http://assos.efrei.fr/robot/ +// Email: robot AT efrei DOT fr +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +// +// }}} +#include "movement_reposition.h" +#include "motor.h" +#include "tracker.h" +#include "date/date.h" + +/// Constructeur. +/// speed : vitesse (-1..+1). +/// gpio : gpio d'entré. +/// gpil : gpi gauche. +/// gpir : gpi droite. +/// ana : analog d'entrée. +/// anal : ana gauche. +/// anar : ana droite. +MovementReposition::MovementReposition (double speed, Gpio &gpio, int gpil, + int gpir, Analog &ana, int anal, int + anar) + : speed_ (speed), gpio_ (gpio), gpil_ (gpil), gpir_ (gpir), ana_ (ana), + anal_ (anal), anar_ (anar), state_ (0) +{ +} + +/// Initialise le mouvement, appelé juste quand le mouvement est mis en +/// service. +void +MovementReposition::init (const Tracker &t, Asserv &a, Motor &m) +{ + Movement::init (t, a, m); + stepDate_ = Date::getInstance ().start (); +} + +/// Controlle la vitesse, retourne faux si mouvement terminé. +bool +MovementReposition::control (void) +{ + std::cout << "reposition: " << state_ << std::endl; + switch (state_) + { + case 0: + { + if (stepDate_ + 5000 < Date::getInstance ().start ()) + return false; + double dl = ana_.get (anal_); + double dr = ana_.get (anar_); + double dm = dl < dr ? dl : dr; + std::cout << "reposition: 0 " << dl << ' ' << dr << std::endl; + m_->speedLR (dl > 62.0 ? speed_ : -speed_ * 0.5, + dr > 62.0 ? speed_ : -speed_ * 0.5, + dm - 62.0, M_PI); + if (dr <= 62.0 && dl <= 62.0) + { + state_ = 1; + stepDate_ = Date::getInstance ().start (); + step1_ = Point (t_->getX (), t_->getY ()); + } + } + return true; + case 1: + { + if (stepDate_ + 5000 < Date::getInstance ().start ()) + return false; + double d = 100.0 - step1_.distTo (Point (t_->getX (), t_->getY ())); + bool l = gpio_.get (gpil_); + bool r = gpio_.get (gpir_); + std::cout << "reposition: 1 " << l << ' ' << r << std::endl; + if (l && r) + { + state_ = 2; + return false; + } + m_->speedLR (!l ? speed_ : -speed_ * 0.05, + !r ? speed_ : -speed_ * 0.05, + d - 50.0, M_PI); + } + return true; + case 2: + { + // XXX: Inutilisé. + bool l = gpio_.get (gpil_); + bool r = gpio_.get (gpir_); + std::cout << "reposition: 2 " << l << ' ' << r << std::endl; + if (!l && !r) + return false; + m_->speed (!l ? speed_ * 0.05 : -speed_ * 0.05, + !r ? speed_ * 0.05 : -speed_ * 0.05); + } + return true; + } + return false; +} + diff --git a/2004/i/nono/src/motor/movement_reposition.h b/2004/i/nono/src/motor/movement_reposition.h new file mode 100644 index 0000000..14a9c50 --- /dev/null +++ b/2004/i/nono/src/motor/movement_reposition.h @@ -0,0 +1,67 @@ +#ifndef movement_reposition_h +#define movement_reposition_h +// movement_reposition.h +// nono - programme du robot 2004. {{{ +// +// Copyright (C) 2004 Nicolas Schodet +// +// Robot APB Team/Efrei 2004. +// Web: http://assos.efrei.fr/robot/ +// Email: robot AT efrei DOT fr +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +// +// }}} +#include "movement.h" +#include "io/gpio.h" +#include "io/analog.h" +#include "utils/point.h" + +/// Mouvement de repositionnement, avance/recule jusqu'à ce que deux gpi soit +/// à 1. +class MovementReposition : public Movement +{ + double speed_; + Gpio &gpio_; + int gpil_, gpir_; + Analog &ana_; + int anal_, anar_; + /// Etat du repositionnement : + /// 0 - raprochement avec les sharps. + /// 1 - raprochement contact. + /// 2 - décontact. + int state_; + // Point de passage à l'étape 1. + Point step1_; + int stepDate_; + public: + /// Constructeur. + /// speed : vitesse (-1..+1). + /// gpio : gpio d'entré. + /// gpil : gpi gauche. + /// gpir : gpi droite. + /// ana : analog d'entrée. + /// anal : ana gauche. + /// anar : ana droite. + MovementReposition (double speed, Gpio &gpio, int gpil, int gpir, + Analog &ana, int anal, int anar); + /// Initialise le mouvement, appelé juste quand le mouvement est mis en + /// service. + void init (const Tracker &t, Asserv &a, Motor &m); + /// Controlle la vitesse, retourne faux si mouvement terminé. + bool control (void); +}; + +#endif // movement_reposition_h diff --git a/2004/i/nono/src/motor/movement_rotation.cc b/2004/i/nono/src/motor/movement_rotation.cc index ee49f3e..a2223ad 100644 --- a/2004/i/nono/src/motor/movement_rotation.cc +++ b/2004/i/nono/src/motor/movement_rotation.cc @@ -76,7 +76,7 @@ std::cout << "movement rotation error " << ea << std::endl; // Commande les moteurs. double a = param_.kpa_ * (ea + param_.kia_ * ia_ + param_.kda_ * (ea - lea_)); - m_->speed (a, -a); + m_->rotate (-a, ea); // Retiens l'erreur pour la dérivée. lea_ = ea; return true; diff --git a/2004/i/nono/src/motor/tracker.cc b/2004/i/nono/src/motor/tracker.cc index 4bd0e7b..a87beaf 100644 --- a/2004/i/nono/src/motor/tracker.cc +++ b/2004/i/nono/src/motor/tracker.cc @@ -29,7 +29,10 @@ /// Constructeur. Tracker::Tracker (void) : posX_ (0.0), posY_ (0.0), angle_ (0.0), - f_ (10.0), zero_ (0), log_ ("tracker") + f_ (10.0), zero_ (0), oldCountLeft_ (0.0), oldCountRight_ (0.0), + adjustXLimit_ (120.0), adjustYLimit_ (120.0), + adjustAngleLimit_ (M_PI/3/2), + log_ ("tracker") { // Lit la conf. Config rc ("rc/tracker"); @@ -39,6 +42,9 @@ Tracker::Tracker (void) rc.get ("startx", posX_) || rc.get ("starty", posY_) || rc.get ("startangle", angle_) || + rc.get ("adjustx", adjustXLimit_) || + rc.get ("adjusty", adjustYLimit_) || + rc.get ("adjustangle", adjustAngleLimit_) || rc.get ("footing", f_) )) rc.noId (); @@ -68,6 +74,30 @@ Tracker::getPos (double &x, double &y, double &angle) const angle = angle_; } +/// Ajuste les X. +void +Tracker::adjustX (double x) +{ + if (fabs (posX_ - x) < adjustXLimit_) + posX_ = x; +} + +/// Ajuste les Y. +void +Tracker::adjustY (double y) +{ + if (fabs (posY_ - y) < adjustYLimit_) + posY_ = y; +} + +/// Ajuste l'angle. +void +Tracker::adjustAngle (double angle) +{ + if (fabs (getAngleDiff (angle)) < adjustAngleLimit_) + angle_ = angle; +} + /// Récupère l'angle entre l'angle courant et a normalisé entre -pi et pi. double Tracker::getAngleDiff (double a) const @@ -219,55 +249,55 @@ Tracker::computeArcs (double x, double y, double &l, double &r, double eps) return true; } -/// Met à jour la position. +/// Met à jour la position (mm). void -Tracker::update (double dL, double dR, bool zero) +Tracker::update (double l, double r, bool zero) { + double dL, dR; + dL = l - oldCountLeft_; + dR = r - oldCountRight_; + oldCountLeft_ = l; + oldCountRight_ = r; + double oldAngle = angle_; + angle_ += (dR - dL) / f_; // Compte les zeros. if (zero) { zero_++; } - else if (dL == 0.0 && dR == 0.0) - { - zero_++; - return; - } else { zero_ = 0; } // Calcule l'angle et l'avancement moyen. // Avec a petit (c'est le cas, car f_ >> abs (dR - dL)), a ~= atan (a). - double dA = (dR - dL) / f_; + // En fait, non, on doit pas calculer l'arctangente... A vérifier. + double dA = angle_ - oldAngle; double dS = 0.5 * (dL + dR); - log_ (Log::verydebug) << "update dL " << dL << " dR " << dR << " dA " << - dA << " dS " << dS << std::endl; +// log_ (Log::verydebug) << "update dL " << dL << " dR " << dR << " dA " << +// dA << " dS " << dS << std::endl; // Si l'angle est petit, évite une division par presque 0. if (dA < 0.0001 && dA > -0.0001) { // Considère que l'on avance en ligne droite selon la moitié de // l'angle. - double a = angle_ + 0.5 * dA; + double a = (angle_ + oldAngle) * 0.5; posX_ += dS * cos (a); posY_ += dS * sin (a); - angle_ += dA; - log_ (Log::verydebug) << "small dX " << dS * cos (a) << " dY " - << dS * sin (a) << std::endl; + /*log_ (Log::verydebug) << "small dX " << dS * cos (a) << " dY " + << dS * sin (a) << std::endl;*/ } else { // dS : arc de cercle parcouru (grâce à Thales). // R : rayon, dS = dA * R, R = dS / dA // On a plus qu'à calculer les dX, dY avec cos et sin. - double oldA = angle_; - angle_ += dA; - posX_ += (sin (angle_) - sin (oldA)) * dS / dA; - posY_ += (cos (oldA) - cos (angle_)) * dS / dA; - log_ (Log::verydebug) << "big dX " << - -(sin (oldA) - sin (angle_)) * dS / dA << - " dY " << -(cos (oldA) - cos (angle_)) * dS / dA << - " R " << dS / dA << std::endl; + posX_ += (sin (angle_) - sin (oldAngle)) * dS / dA; + posY_ += (cos (oldAngle) - cos (angle_)) * dS / dA; + /*log_ (Log::verydebug) << "big dX " << + (sin (angle_) - sin (oldAngle)) * dS / dA << + " dY " << (cos (oldAngle) - cos (angle_)) * dS / dA << + " R " << dS / dA << std::endl;*/ } log_ (Log::info) << "update pos " << *this << std::endl; } diff --git a/2004/i/nono/src/motor/tracker.h b/2004/i/nono/src/motor/tracker.h index 96cf6f0..484329a 100644 --- a/2004/i/nono/src/motor/tracker.h +++ b/2004/i/nono/src/motor/tracker.h @@ -38,6 +38,10 @@ class Tracker double f_; /// Nombre d'updates nuls consécutifs. int zero_; + /// Anciène valeur des compteurs. + double oldCountLeft_, oldCountRight_; + /// Limite admissible pour les adjusts. + double adjustXLimit_, adjustYLimit_, adjustAngleLimit_; // Système de log. mutable Log log_; public: @@ -55,6 +59,12 @@ class Tracker double getY (void) const { return posY_; } /// Get angle. double getAngle (void) const { return angle_; } + /// Ajuste les X. + void adjustX (double x); + /// Ajuste les Y. + void adjustY (double y); + /// Ajuste l'angle. + void adjustAngle (double angle); /// Get distance. double getDistance (double x, double y) const; /// Récupère la distance à parcourir avec chaque roue pour parcourir un @@ -85,8 +95,8 @@ class Tracker /// \deprecated Pas vraiment l'algo qui faut. bool computeArcs (double x, double y, double &l, double &r, double eps) const; - /// Met à jour la position. - void update (double dL, double dR, bool zero); + /// Met à jour la position (mm). + void update (double l, double r, bool zero); }; /// Affiche la position. -- cgit v1.2.3