From 9fdd6704947174fae292dfc6e14cc1029a7f7a6c Mon Sep 17 00:00:00 2001 From: schodet Date: Sat, 7 Feb 2004 16:55:41 +0000 Subject: Initial revision --- 2004/i/nono/src/motor/movement_goto.cc | 68 ++++++++++++++++++++++++++++++++++ 1 file changed, 68 insertions(+) create mode 100644 2004/i/nono/src/motor/movement_goto.cc (limited to '2004/i/nono/src/motor/movement_goto.cc') diff --git a/2004/i/nono/src/motor/movement_goto.cc b/2004/i/nono/src/motor/movement_goto.cc new file mode 100644 index 0000000..69c1b91 --- /dev/null +++ b/2004/i/nono/src/motor/movement_goto.cc @@ -0,0 +1,68 @@ +// movement_goto.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_goto.h" +#include "tracker.h" +#include "motor.h" + +// 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), + il_ (0.0), ia_ (0.0), lel_ (0.0), lea_ (0.0) +{ +} + +// Controlle la vitesse, retourne faux si mouvement terminé. +bool +MovementGoTo::control (void) +{ + // Calcule l'erreur. + double el, ea; + if (!t_->computeError (dX_, dY_, el, ea, eps_, dist_)) + { + return false; + } + // Pas de marche arrière. + if (el < 0.0) el = 0.0; +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; + ia_ += ea; + if (ia_ > 1000) ia_ = 1000; + else if (ia_ < -1000) ia_ = -1000; + // Commande les moteurs. + double l = kpl_ * (el + kil_ * il_ + kdl_ * (el - lel_)); + double a = kpa_ * (ea + kia_ * ia_ + kda_ * (ea - lea_)); + m_->speed (l - a, l + a); + // Retiens l'erreur pour la dérivée. + lel_ = el; + lea_ = ea; + return true; +} + -- cgit v1.2.3