summaryrefslogtreecommitdiff
path: root/2004
diff options
context:
space:
mode:
authorschodet2004-06-27 14:36:36 +0000
committerschodet2004-06-27 14:36:36 +0000
commitd4adf20d86ce0e1e887523042d79e22a5930d954 (patch)
tree9c7d7bac833fef3288db5f4ac1cbe8a369147463 /2004
parentb097c16be082ddd505db55bf9531d98cb2eadc65 (diff)
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.
Diffstat (limited to '2004')
-rw-r--r--2004/i/nono/src/motor/asserv.cc33
-rw-r--r--2004/i/nono/src/motor/asserv.h12
-rw-r--r--2004/i/nono/src/motor/goto_tracker.cc16
-rw-r--r--2004/i/nono/src/motor/motor.cc28
-rw-r--r--2004/i/nono/src/motor/motor.h9
-rw-r--r--2004/i/nono/src/motor/motor_cmd.cc13
-rw-r--r--2004/i/nono/src/motor/movement_goto.cc7
-rw-r--r--2004/i/nono/src/motor/movement_reposition.cc114
-rw-r--r--2004/i/nono/src/motor/movement_reposition.h67
-rw-r--r--2004/i/nono/src/motor/movement_rotation.cc2
-rw-r--r--2004/i/nono/src/motor/tracker.cc76
-rw-r--r--2004/i/nono/src/motor/tracker.h14
12 files changed, 337 insertions, 54 deletions
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 paramtres.
+ 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<int> (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
// Paramtres.
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<std::string> sendQueue_;
+ static const int recvTimeout_ = 500;
+ int lastSent_;
// Buffer de reception.
int inBufSize_, inBufPos_;
char *inBuf_;
// Ancinne valeur des compteur.
bool firstCounter_;
int countLeft_, countRight_;
- // Valeur de l'entre GPI.
+ int countLeftSum_, countRightSum_;
+ // Valeurs GPIO.
unsigned int gpi_;
+ unsigned int gpo_;
// Objet interess par les stats.
AsservTracker &asservTracker_;
// Systme 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);
+}
+
+/// Paramtre la vitesse linaire (-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);
+}
+
+/// Paramtre 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);
}
/// Paramtre 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 arrts.
void waitStopped (void);
/// Rcupre l'object tracker.
+ Tracker &getTracker (void) { return tracker_; }
+ /// Rcupre l'object tracker.
const Tracker &getTracker (void) const { return tracker_; }
/// Rcupre 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);
+ /// Paramtre la vitesse linaire (-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);
+ /// Paramtre la vitesse angulaire (-1..+1). Limite la vitesse pour
+ /// pouvoir freiner au bout de l'angle (rad).
+ void rotate (double a, double angle);
/// Paramtre la vitesse des moteurs (-1..+1).
void speed (double l, double r);
/// Paramtre 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 <x> <y> va en (x, y)\n"
" r <a> se dirige vers l'angle a en degrs\n"
+ " R <v> Tourne la vitesse v\n"
" c <r> <a> Suis un arc de rayon r et d'angle a en degrs\n"
" h <fa> [p <x> <y>]...\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 intgrales satures.
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 drive.
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'entre.
+/// 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 - dcontact.
+ 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'entre.
+ /// 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 drive.
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;
+}
+
/// Rcupre 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 vrifier.
+ 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)
{
// Considre 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 (grce 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 conscutifs.
int zero_;
+ /// Ancine valeur des compteurs.
+ double oldCountLeft_, oldCountRight_;
+ /// Limite admissible pour les adjusts.
+ double adjustXLimit_, adjustYLimit_, adjustAngleLimit_;
// Systme 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;
/// Rcupre 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.