summaryrefslogtreecommitdiff
path: root/2004/i/nono/src/motor/movement_rotation.cc
diff options
context:
space:
mode:
authorschodet2004-04-16 16:46:46 +0000
committerschodet2004-04-16 16:46:46 +0000
commita3a3a4d6d183b22036a606cda8553192c98321cd (patch)
tree37d158408eabfbb8524d1214a985a44e376ee333 /2004/i/nono/src/motor/movement_rotation.cc
parente68b824329a537eddddd5b59b9bdf498698ee604 (diff)
Logger fonctionne.
MovementRotation aussi. Ajout de Serial::wait. Ajout de commentaires.
Diffstat (limited to '2004/i/nono/src/motor/movement_rotation.cc')
-rw-r--r--2004/i/nono/src/motor/movement_rotation.cc84
1 files changed, 84 insertions, 0 deletions
diff --git a/2004/i/nono/src/motor/movement_rotation.cc b/2004/i/nono/src/motor/movement_rotation.cc
new file mode 100644
index 0000000..2f58e45
--- /dev/null
+++ b/2004/i/nono/src/motor/movement_rotation.cc
@@ -0,0 +1,84 @@
+// movement_rotation.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_rotation.h"
+#include "tracker.h"
+#include "motor.h"
+#include "config/config.h"
+
+MovementRotationParam MovementRotation::param_;
+
+/// Constructeur, charge les paramètres depuis la Config.
+MovementRotationParam::MovementRotationParam (void)
+ : eps_ (0.01),
+ kpa_ (1.0), kia_ (0.0), is_ (1000.0),
+ kda_ (0.0)
+{
+ // Lit la conf.
+ Config rc ("rc/movement/rotation");
+ while (!rc.eof ())
+ {
+ if (!(
+ rc.get ("epsilon", eps_) ||
+ rc.get ("kpa", kpa_) ||
+ rc.get ("kia", kia_) ||
+ rc.get ("is", is_) ||
+ rc.get ("kda", kda_)
+ ))
+ rc.noId ();
+ }
+}
+
+/// Constructeur.
+/// a : angle d'arrivé (rad).
+MovementRotation::MovementRotation (double a)
+ : dA_ (a),
+ ia_ (0.0), lea_ (0.0)
+{
+}
+
+/// Controlle la vitesse, retourne faux si mouvement terminé.
+bool
+MovementRotation::control (void)
+{
+ // Calcule l'erreur.
+ double ea;
+ if (!t_->computeAngleError (dA_, ea, param_.eps_))
+ {
+ return false;
+ }
+std::cout << "movement rotation error " << ea << std::endl;
+ // Calcule les intégrales saturées.
+ ia_ += ea;
+ if (ia_ > param_.is_) ia_ = param_.is_;
+ else if (ia_ < -param_.is_) ia_ = -param_.is_;
+ // Commande les moteurs.
+ double a = param_.kpa_ * (ea + param_.kia_ * ia_
+ + param_.kda_ * (ea - lea_));
+ m_->speed (a, -a);
+ // Retiens l'erreur pour la dérivée.
+ lea_ = ea;
+ return true;
+}
+