From 89d5a4bb3a5ceec0d9d791cba718fec178001a49 Mon Sep 17 00:00:00 2001 From: schodet Date: Sat, 28 Feb 2004 18:42:11 +0000 Subject: Add: paramètres d'asservissement d'un MovementGoto depuis la Config. --- 2004/i/nono/runtime/rc/movement/goto | 4 ++ 2004/i/nono/src/motor/movement_goto.cc | 87 +++++++++++++++++++++++++++++----- 2004/i/nono/src/motor/movement_goto.h | 21 ++++++-- 2004/i/nono/src/motor/test_motor.cc | 5 +- 4 files changed, 95 insertions(+), 22 deletions(-) create mode 100644 2004/i/nono/runtime/rc/movement/goto (limited to '2004') diff --git a/2004/i/nono/runtime/rc/movement/goto b/2004/i/nono/runtime/rc/movement/goto new file mode 100644 index 0000000..b0aac2b --- /dev/null +++ b/2004/i/nono/runtime/rc/movement/goto @@ -0,0 +1,4 @@ +epsilon 10 +kpl 1.0 kil 0.1 kdl 0.7 +kpa 1.5 kia 0.1 kda 0.7 +is 1000 diff --git a/2004/i/nono/src/motor/movement_goto.cc b/2004/i/nono/src/motor/movement_goto.cc index 69c1b91..1b0a683 100644 --- a/2004/i/nono/src/motor/movement_goto.cc +++ b/2004/i/nono/src/motor/movement_goto.cc @@ -25,24 +25,83 @@ #include "movement_goto.h" #include "tracker.h" #include "motor.h" +#include "config/config.h" + +MovementGotoParam MovementGoto::param_; + +/// Constructeur, charge les paramètres depuis la Config. +MovementGotoParam::MovementGotoParam (void) + : eps_ (10.0), dist_ (0.0), + kpl_ (1.0), kpa_ (1.0), kil_ (0.0), kia_ (0.0), is_ (1000.0), + kdl_ (0.0), kda_ (0.0) +{ + // Lit la conf. + Config rc ("rc/movement/goto"); + while (!rc.eof ()) + { + if (rc.isId ("epsilon")) + { + rc.getId (); + rc >> eps_;; + } + else if (rc.isId ("distance")) + { + rc.getId (); + rc >> dist_; + } + else if (rc.isId ("kpl")) + { + rc.getId (); + rc >> kpl_; + } + else if (rc.isId ("kpa")) + { + rc.getId (); + rc >> kpa_; + } + else if (rc.isId ("kil")) + { + rc.getId (); + rc >> kil_; + } + else if (rc.isId ("kia")) + { + rc.getId (); + rc >> kia_; + } + else if (rc.isId ("is")) + { + rc.getId (); + rc >> is_; + } + else if (rc.isId ("kdl")) + { + rc.getId (); + rc >> kdl_; + } + else if (rc.isId ("kda")) + { + rc.getId (); + rc >> kda_; + } + else rc.noId (); + } +} // Constructeur. -MovementGoTo::MovementGoTo (double x, double y, double eps, double dist, - double kpl, double kpa, double kil, double kia, - double kdl, double kda) - : dX_ (x), dY_ (y), eps_ (eps), dist_ (dist), - kpl_ (kpl), kpa_ (kpa), kil_ (kil), kia_ (kia), kdl_ (kdl), kda_ (kda), +MovementGoto::MovementGoto (double x, double y) + : dX_ (x), dY_ (y), il_ (0.0), ia_ (0.0), lel_ (0.0), lea_ (0.0) { } // Controlle la vitesse, retourne faux si mouvement terminé. bool -MovementGoTo::control (void) +MovementGoto::control (void) { // Calcule l'erreur. double el, ea; - if (!t_->computeError (dX_, dY_, el, ea, eps_, dist_)) + if (!t_->computeError (dX_, dY_, el, ea, param_.eps_, param_.dist_)) { return false; } @@ -51,14 +110,16 @@ MovementGoTo::control (void) std::cout << "movement goto error " << el << ' ' << ea << std::endl; // Calcule les intégrales saturées. il_ += el; - if (il_ > 1000) il_ = 1000; - else if (il_ < -1000) il_ = -1000; + if (il_ > param_.is_) il_ = param_.is_; + else if (il_ < -param_.is_) il_ = -param_.is_; ia_ += ea; - if (ia_ > 1000) ia_ = 1000; - else if (ia_ < -1000) ia_ = -1000; + if (ia_ > param_.is_) ia_ = param_.is_; + else if (ia_ < -param_.is_) ia_ = -param_.is_; // Commande les moteurs. - double l = kpl_ * (el + kil_ * il_ + kdl_ * (el - lel_)); - double a = kpa_ * (ea + kia_ * ia_ + kda_ * (ea - lea_)); + double l = param_.kpl_ * (el + param_.kil_ * il_ + + param_.kdl_ * (el - lel_)); + double a = param_.kpa_ * (ea + param_.kia_ * ia_ + + param_.kda_ * (ea - lea_)); m_->speed (l - a, l + a); // Retiens l'erreur pour la dérivée. lel_ = el; diff --git a/2004/i/nono/src/motor/movement_goto.h b/2004/i/nono/src/motor/movement_goto.h index dbd4e67..721d481 100644 --- a/2004/i/nono/src/motor/movement_goto.h +++ b/2004/i/nono/src/motor/movement_goto.h @@ -26,10 +26,8 @@ // }}} #include "movement.h" -class MovementGoTo : public Movement +class MovementGotoParam { - // Position d'arrivée. - double dX_, dY_; // Epsilon. double eps_; // Distance du point cible courant. @@ -38,16 +36,29 @@ class MovementGoTo : public Movement double kpl_, kpa_; // Coefficient intégrals linéaire et angulaire. double kil_, kia_; + // Maximum de saturation de l'integralle. + double is_; // Coefficient de dérivée linéaire et angulaire. double kdl_, kda_; + public: + /// Constructeur, charge les paramètres depuis la Config. + MovementGotoParam (void); + friend class MovementGoto; +}; + +class MovementGoto : public Movement +{ + // Position d'arrivée. + double dX_, dY_; // Integrales. double il_, ia_; // Dernière valeurs d'erreur. double lel_, lea_; + // Paramètres d'asservissement. + static MovementGotoParam param_; public: // Constructeur. - MovementGoTo (double x, double y, double eps, double dist, double kpl, - double kpa, double kil, double kia, double kdl, double kda); + MovementGoto (double x, double y); // Controlle la vitesse, retourne faux si mouvement terminé. bool control (void); }; diff --git a/2004/i/nono/src/motor/test_motor.cc b/2004/i/nono/src/motor/test_motor.cc index 325975e..40ddb99 100644 --- a/2004/i/nono/src/motor/test_motor.cc +++ b/2004/i/nono/src/motor/test_motor.cc @@ -83,10 +83,7 @@ main (int argc, char **argv) if (i >= argc) break; dY = atof (argv[i++]); cout << "goto " << dX << ' ' << dY << endl; - Movement *mov = new MovementGoTo (dX, dY, 10.0, 0.0, - 1.0, 1.5, - 0.1, 0.1, - 0.7, 0.7); + Movement *mov = new MovementGoto (dX, dY); m.setMovement (mov); } break; -- cgit v1.2.3