summaryrefslogtreecommitdiff
path: root/2004/i
diff options
context:
space:
mode:
authorschodet2004-04-25 20:48:52 +0000
committerschodet2004-04-25 20:48:52 +0000
commit3af9c88516543659002f5303d703f32579b45d6f (patch)
tree0ffefb8dd07b9a748e5e8338d673fee94f3b706b /2004/i
parentba4f2fedc890eb3a565c5d92cce6e88d2134e2ab (diff)
Modification de Goto.
Diffstat (limited to '2004/i')
-rw-r--r--2004/i/nono/src/motor/Makefile.defs3
-rw-r--r--2004/i/nono/src/motor/goto.h17
-rw-r--r--2004/i/nono/src/motor/goto_simple.cc23
-rw-r--r--2004/i/nono/src/motor/goto_simple.h14
-rw-r--r--2004/i/nono/src/motor/motor.cc8
-rw-r--r--2004/i/nono/src/motor/motor.h3
-rw-r--r--2004/i/nono/src/motor/movement_goto.cc31
-rw-r--r--2004/i/nono/src/motor/movement_goto.h3
-rw-r--r--2004/i/nono/src/motor/movement_types.h1
-rw-r--r--2004/i/nono/src/motor/test_motor.cc31
-rw-r--r--2004/i/nono/src/motor/tracker.cc33
-rw-r--r--2004/i/nono/src/motor/tracker.h4
12 files changed, 118 insertions, 53 deletions
diff --git a/2004/i/nono/src/motor/Makefile.defs b/2004/i/nono/src/motor/Makefile.defs
index a273875..84ea47d 100644
--- a/2004/i/nono/src/motor/Makefile.defs
+++ b/2004/i/nono/src/motor/Makefile.defs
@@ -7,7 +7,8 @@ test_asserv_SOURCES = test_asserv.cc motor.a serial.a erreur.a config.a \
test_tracker_SOURCES = test_tracker.cc motor.a serial.a erreur.a config.a \
date.a utils.a logger.a
motor_a_SOURCES = asserv.cc tracker.cc motor.cc movement_basic.cc \
- movement_goto.cc movement_rotation.cc goto_simple.cc
+ movement_goto.cc movement_rotation.cc goto_simple.cc \
+ goto_circle.cc
test_motor: $(test_motor_SOURCES:%.cc=%.o)
diff --git a/2004/i/nono/src/motor/goto.h b/2004/i/nono/src/motor/goto.h
index e062c5d..af6cedc 100644
--- a/2004/i/nono/src/motor/goto.h
+++ b/2004/i/nono/src/motor/goto.h
@@ -32,14 +32,17 @@ class Goto
public:
/// Destructeur.
virtual ~Goto (void) { }
- /// Fournit la distance au point final (mm) et renvois false si le Goto
- /// est fini.
- /// eps: distance (mm) en dessous de laquelle on considère que la
+ /// Initialise le Goto, appelé au début de la trajectoire.
+ virtual void init (const Tracker &t) { }
+ /// Fournit la distance au point final (mm), le point le plus loin à moins
+ /// de distmax (mm) et renvois false si le Goto est fini.
+ /// eps : distance (mm) en dessous de laquelle on considère que la
/// destination est atteinte.
- virtual bool getDistance (const Tracker &t, double &dist, double eps) = 0;
- /// Fournit le point le plus loin à moins de dist (mm).
- virtual void getPoint (const Tracker &t, double &dx, double &dy,
- double dist) = 0;
+ /// distmax : distance (mm) maximale du point.
+ /// dist : distance (mm) au point final.
+ /// (x, y) : point intermédiaire (mm).
+ virtual bool get (const Tracker &t, double distmax, double eps,
+ double &dist, double &x, double &y) = 0;
};
#endif // goto_h
diff --git a/2004/i/nono/src/motor/goto_simple.cc b/2004/i/nono/src/motor/goto_simple.cc
index fd6a16e..ffc23ce 100644
--- a/2004/i/nono/src/motor/goto_simple.cc
+++ b/2004/i/nono/src/motor/goto_simple.cc
@@ -31,22 +31,19 @@ GotoSimple::GotoSimple (double dx, double dy)
{
}
-/// Fournit la distance au point final (mm) et renvois false si le Goto est
-/// fini.
-/// eps: distance (mm) en dessous de laquelle on considère que la
-/// destination est atteinte.
+/// Fournit la distance au point final (mm), le point le plus loin à moins
+/// de distmax (mm) et renvois false si le Goto est fini.
+/// eps : distance (mm) en dessous de laquelle on considère que la destination
+/// est atteinte.
+/// distmax : distance (mm) maximale du point.
+/// dist : distance (mm) au point final.
+/// (x, y) : point intermédiaire (mm).
bool
-GotoSimple::getDistance (const Tracker &t, double &dist, double eps)
+GotoSimple::get (const Tracker &t, double distmax, double eps, double &dist,
+ double &x, double &y)
{
dist = t.getDistance (dx_, dy_);
+ t.getPoint (dx_, dy_, x, y, distmax);
return dist > eps;
}
-/// Fournit le point le plus loin à moins de dist (mm).
-void
-GotoSimple::getPoint (const Tracker &t, double &dx, double &dy, double dist)
-{
- dx = dx_;
- dy = dy_;
-}
-
diff --git a/2004/i/nono/src/motor/goto_simple.h b/2004/i/nono/src/motor/goto_simple.h
index 950203e..d773720 100644
--- a/2004/i/nono/src/motor/goto_simple.h
+++ b/2004/i/nono/src/motor/goto_simple.h
@@ -34,13 +34,15 @@ class GotoSimple : public Goto
/// Constructeur.
/// (dx, dy) : position d'arrivée (mm).
GotoSimple (double dx, double dy);
- /// Fournit la distance au point final (mm) et renvois false si le Goto
- /// est fini.
- /// eps: distance (mm) en dessous de laquelle on considère que la
+ /// Fournit la distance au point final (mm), le point le plus loin à moins
+ /// de distmax (mm) et renvois false si le Goto est fini.
+ /// eps : distance (mm) en dessous de laquelle on considère que la
/// destination est atteinte.
- bool getDistance (const Tracker &t, double &dist, double eps);
- /// Fournit le point le plus loin à moins de dist (mm).
- void getPoint (const Tracker &t, double &dx, double &dy, double dist);
+ /// distmax : distance (mm) maximale du point.
+ /// dist : distance (mm) au point final.
+ /// (x, y) : point intermédiaire (mm).
+ bool get (const Tracker &t, double distmax, double eps,
+ double &dist, double &x, double &y);
};
#endif // goto_simple_h
diff --git a/2004/i/nono/src/motor/motor.cc b/2004/i/nono/src/motor/motor.cc
index 7bbae46..45c9955 100644
--- a/2004/i/nono/src/motor/motor.cc
+++ b/2004/i/nono/src/motor/motor.cc
@@ -122,6 +122,14 @@ Motor::waitStopped (void)
} while (!stopped ());
}
+/// Paramètre la vitesse linéaire (-1..+1) et angulaire (-1..+1). Limite
+/// la vitesse pour pouvoir freiner au bout de dist (mm).
+void
+Motor::speed (double l, double a, double dist)
+{
+ speed (computeSpeed (dist) * (l - a), computeSpeed (dist) * (l + a));
+}
+
/// Paramètre la vitesse des moteurs (-1..+1).
void
Motor::speed (double l, double r)
diff --git a/2004/i/nono/src/motor/motor.h b/2004/i/nono/src/motor/motor.h
index cd596a7..a5e15db 100644
--- a/2004/i/nono/src/motor/motor.h
+++ b/2004/i/nono/src/motor/motor.h
@@ -76,6 +76,9 @@ class Motor : public AsservTracker
void waitStopped (void);
/// Récupère l'object tracker.
const Tracker &getTracker (void) const { return tracker_; }
+ /// Paramètre la vitesse linéaire (-1..+1) et angulaire (-1..+1). Limite
+ /// la vitesse pour pouvoir freiner au bout de dist (mm).
+ void speed (double l, double a, double dist);
/// Paramètre la vitesse des moteurs (-1..+1).
void speed (double l, double r);
/// Calcule la vitesse en ligne droit afin de pouvoir freiner au bout
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;
diff --git a/2004/i/nono/src/motor/movement_goto.h b/2004/i/nono/src/motor/movement_goto.h
index 95dde47..acbb3e1 100644
--- a/2004/i/nono/src/motor/movement_goto.h
+++ b/2004/i/nono/src/motor/movement_goto.h
@@ -67,6 +67,9 @@ class MovementGoto : public Movement
MovementGoto (Goto *go);
/// Destructeur.
~MovementGoto (void);
+ /// 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);
};
diff --git a/2004/i/nono/src/motor/movement_types.h b/2004/i/nono/src/motor/movement_types.h
index 66ad1d0..7fb1ec4 100644
--- a/2004/i/nono/src/motor/movement_types.h
+++ b/2004/i/nono/src/motor/movement_types.h
@@ -28,5 +28,6 @@
#include "movement_goto.h"
#include "movement_rotation.h"
#include "goto_simple.h"
+#include "goto_circle.h"
#endif // movement_types_h
diff --git a/2004/i/nono/src/motor/test_motor.cc b/2004/i/nono/src/motor/test_motor.cc
index 27d4e68..42130bf 100644
--- a/2004/i/nono/src/motor/test_motor.cc
+++ b/2004/i/nono/src/motor/test_motor.cc
@@ -45,7 +45,8 @@ syntax (void)
" b <speed> <len>\n"
" mouvement basique\n"
" t <x> <y> va en (x, y)\n"
- " r <a> se dirige vers l'angle a en degrées\n"
+ " r <a> se dirige vers l'angle a en degrés\n"
+ " c <r> <a> Suis un arc de rayon r et d'angle a en degrés\n"
" <timeout> attend un certain temps (qui augmente avec timeout)\n"
" ? cet ecran d'aide" << std::endl;
}
@@ -62,17 +63,17 @@ main (int argc, char **argv)
switch (argv[i][0])
{
case 'g':
- cout << "go" << endl;
+ cout << "test: go" << endl;
i++;
m.go ();
break;
case 'G':
- cout << "ungo" << endl;
+ cout << "test: ungo" << endl;
i++;
m.go (false);
break;
case 'w':
- cout << "wait stop" << endl;
+ cout << "test: wait stop" << endl;
i++;
m.waitStopped ();
break;
@@ -85,7 +86,7 @@ main (int argc, char **argv)
speed = atof (argv[i++]);
if (i >= argc) break;
len = atof (argv[i++]);
- cout << "basic " << speed << ' ' << len << endl;
+ cout << "test: basic " << speed << ' ' << len << endl;
Movement *mov = new MovementBasic (speed, len);
m.setMovement (mov);
}
@@ -98,7 +99,7 @@ main (int argc, char **argv)
dX = atof (argv[i++]);
if (i >= argc) break;
dY = atof (argv[i++]);
- cout << "goto " << dX << ' ' << dY << endl;
+ cout << "test: goto " << dX << ' ' << dY << endl;
Goto *g = new GotoSimple (dX, dY);
Movement *mov = new MovementGoto (g);
m.setMovement (mov);
@@ -110,12 +111,26 @@ main (int argc, char **argv)
double dA;
if (i >= argc) break;
dA = atof (argv[i++]);
- cout << "rotation " << dA << endl;
+ cout << "test: rotation " << dA << endl;
Movement *mov = new MovementRotation (dA * 2 * M_PI /
360);
m.setMovement (mov);
}
break;
+ case 'c':
+ {
+ i++;
+ double r, a;
+ if (i >= argc) break;
+ r = atof (argv[i++]);
+ if (i >= argc) break;
+ a = atof (argv[i++]);
+ cout << "test: circle " << r << ' ' << a << endl;
+ Goto *g = new GotoCircle (r, a * 2 * M_PI / 360);
+ Movement *mov = new MovementGoto (g);
+ m.setMovement (mov);
+ }
+ break;
case '?':
syntax ();
return 0;
@@ -127,7 +142,7 @@ main (int argc, char **argv)
syntax ();
return 1;
}
- cout << "sleep " << s << endl;
+ cout << "test: sleep " << s << endl;
for (int j = 0; j < s * 1000 / 50; ++j)
{
m.ok ();
diff --git a/2004/i/nono/src/motor/tracker.cc b/2004/i/nono/src/motor/tracker.cc
index 4ec28b9..270455a 100644
--- a/2004/i/nono/src/motor/tracker.cc
+++ b/2004/i/nono/src/motor/tracker.cc
@@ -24,8 +24,7 @@
// }}}
#include "tracker.h"
#include "config/config.h"
-
-#include <math.h>
+#include "utils/mathutil.h"
/// Constructeur.
Tracker::Tracker (void)
@@ -73,12 +72,30 @@ Tracker::getPos (double &x, double &y, double &angle) const
double
Tracker::getAngleDiff (double a) const
{
- double diff = angle_ - a;
- while (diff > M_PI)
- diff -= M_PI;
- while (diff < -M_PI)
- diff += M_PI;
- return diff;
+ return angleNorm (angle_ - a);
+}
+
+/// Récupère les coordonnées d'un point à moins de dist (mm), dans la
+/// direction de (dx, dy).
+void
+Tracker::getPoint (double dx, double dy, double &x, double &y, double dist)
+ const
+{
+ // Calcule le delta.
+ double dtx = dx - posX_;
+ double dty = dy - posY_;
+ // Test si trops loin.
+ double sqd = dtx * dtx + dty * dty;
+ if (sqd > dist * dist)
+ {
+ x = posX_ + dtx * dist / sqrt (sqd);
+ y = posY_ + dty * dist / sqrt (sqd);
+ }
+ else
+ {
+ x = dx;
+ y = dy;
+ }
}
/// Calcule l'erreur linéaire et angulaire pour atteindre un point.
diff --git a/2004/i/nono/src/motor/tracker.h b/2004/i/nono/src/motor/tracker.h
index 9648e3e..5e80a65 100644
--- a/2004/i/nono/src/motor/tracker.h
+++ b/2004/i/nono/src/motor/tracker.h
@@ -57,6 +57,10 @@ class Tracker
double getAngle (void) const { return angle_; }
/// Get distance.
double getDistance (double x, double y) const;
+ /// Récupère les coordonnées d'un point à moins de dist (mm), dans la
+ /// direction de (dx, dy).
+ void getPoint (double dx, double dy, double &x, double &y, double dist)
+ const;
/// Retourne vrai si arrété.
bool stopped (void) { return zero_ > 10; }
/// Récupère l'angle entre l'angle courant et a normalisé entre -pi et pi.