// 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 #include /// Constructeur. GpioServoPp::GpioServoPp (void) : outputs_ (0), inputs_ (0), 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; } /// 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; } /// Bouge un servo de DIFF. void GpioServoPp::move (int n, int diff) { n %= nbServo_; 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 %= nbServo_; servoPos_[n] = pos; int v = n << 7 | servoPos_[n]; serial_.putchar (v); log_ (Log::debug) << "send " << std::hex << v << std::dec << std::endl; } /// Met à jour. void GpioServoPp::update (void) { int c; while ((c = serial_.getchar ()) != -1) { log_ (Log::debug) << "recv " << std::hex << c << std::dec << std::endl; inputs_ = c; } } /// Récupère le nombre d'entrées/sorties. int GpioServoPp::getNbIo (void) { return nbOutputs_ + nbInputs_; }