summaryrefslogtreecommitdiff
path: root/2004/i/nono/src/io/gpio_servo_pp.cc
diff options
context:
space:
mode:
Diffstat (limited to '2004/i/nono/src/io/gpio_servo_pp.cc')
-rw-r--r--2004/i/nono/src/io/gpio_servo_pp.cc175
1 files changed, 175 insertions, 0 deletions
diff --git a/2004/i/nono/src/io/gpio_servo_pp.cc b/2004/i/nono/src/io/gpio_servo_pp.cc
new file mode 100644
index 0000000..36f9334
--- /dev/null
+++ b/2004/i/nono/src/io/gpio_servo_pp.cc
@@ -0,0 +1,175 @@
+// gpio_servo_pp.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 "gpio_servo_pp.h"
+#include "config/config.h"
+
+#include <iomanip>
+
+/// Constructeur.
+GpioServoPp::GpioServoPp (void)
+ : outputs_ (0), inputs_ (0), lastInputRequestSend_ (-10000),
+ lastInputRecv_ (-10000), serial_ (true), ttyspeed_ (0),
+ log_ ("io", "pp")
+{
+ Config rc ("rc/gpio_servo_pp");
+ while (!rc.eof ())
+ {
+ if (!(
+ rc.get ("tty", ttyname_) ||
+ rc.get ("ttyspeed", ttyspeed_) ||
+ ))
+ rc.noId ();
+ }
+ serial_.open (ttyname_, ttyspeed_);
+ for (int i = 0; i < nbServo_; ++i)
+ servoPos_[i] = 0;
+}
+
+/// Destructeur.
+GpioServoPp::~GpioServoPp (void)
+{
+}
+
+/// Change plusieurs sorties.
+void
+GpioServoPp::set (int n, unsigned int bits, int nb)
+{
+ if (n < 0 || n + nb > nbOutputs_)
+ throw std::out_of_range ("GpioServoPp::set: Out of range.");
+ unsigned int mask = (1 << nb) - 1;
+ bits <<= n;
+ outputs_ = (outputs_ & ~mask) | (bits & mask) | 0x80;
+ serial.putchar (outputs_);
+ log_ (Log::debug) << "send " << std::hex << outputs_ << std::dec <<
+ std::endl;
+}
+
+/// Change une sortie.
+void
+GpioServoPp::set (int n, unsigned int bit)
+{
+ set (n, bit, 1);
+}
+
+/// Lit plusieurs entrées.
+unsigned int
+GpioServoPp::get (int n, int nb)
+{
+ if (n < nbOutputs_ || n + nb > nbOutputs_ + nbInputs_)
+ throw std::out_of_range ("GpioServoPp::get: Out of range.");
+ unsigned int mask = 1 << (nb) - 1;
+ unsigned int bits = (inputs_ >> (n - nbOutputs_)) & mask;
+ return bits;
+}
+
+/// Lit une entrée.
+unsigned int
+GpioServoPp::get (int n)
+{
+ return get (n, 1);
+}
+
+/// Pré-lit plusieurs entrées.
+bool
+GpioServoPp::preget (int n, int nb)
+{
+ int date = Date::getInstance ().start ();
+ if (lastInputRecv_ + inputValidityTimeout_ > date)
+ {
+ // Donnée prête.
+ return true;
+ }
+ else
+ {
+ // Donnée périmée.
+ // Lit tout ce qui arrive du port serie.
+ int c;
+ while ((c = serial_.getchar ()) != -1)
+ {
+ log_ (Log::debug) << "recv " << std::hex << c << std::dec <<
+ std::endl;
+ inputs_ = c;
+ lastInputRecv_ = date;
+ }
+ // Si le dernier envois est assez recent, on garde ce qu'on a recu.
+ if (lastInputRequestSend_ + inputRequestTimeout_ > date)
+ {
+ return true;
+ }
+ else
+ {
+ lastInputRecv_ = -10000;
+ lastInputRequestSend_ = date;
+ serial_.putchar (0xff);
+ log_ (Log::debug) << "recv " << std::hex << 0xff << std::dec <<
+ std::endl;
+ return false;
+ }
+ }
+}
+
+/// Pré-lit une entrée.
+bool
+GpioServoPp::preget (int n)
+{
+ return preget (n, 1);
+}
+
+/// Change la direction de plusieurs I/O.
+void
+GpioServoPp::dir (int n, unsigned int bits, int nb)
+{
+ throw std::invalid_argument ("GpioServoPp::dir: Operation not supported");
+}
+
+/// Change la direction d'un I/O (1 pour I, 0 pour O).
+void
+GpioServoPp::dir (int n, unsigned int bit)
+{
+ dir (n, bit, 1);
+}
+
+/// Bouge un servo de DIFF.
+void
+GpioServoPp::move (int n, int diff)
+{
+ n %= servoPos_;
+ servoPos_[n] += diff;
+ int v = n << 7 | servoPos_[n];
+ serial.putchar (v);
+ log_ (Log::debug) << "send " << std::hex << v << std::dec << std::endl;
+}
+
+/// Bouge un servo en position POS.
+void
+GpioServoPp::moveTo (int n, int pos)
+{
+ n %= servoPos_;
+ servoPos_[n] = pos;
+ int v = n << 7 | servoPos_[n];
+ serial.putchar (v);
+ log_ (Log::debug) << "send " << std::hex << v << std::dec << std::endl;
+}
+