// 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), 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 (); } // Ouvre le port série. serial_.open (ttyname_, ttyspeed_); for (int i = 0; i < nbServo_; ++i) servoPos_[i] = 0; // Construit la table des noms. names_["o0"] = 0; names_["o1"] = 1; names_["o2"] = 2; names_["o3"] = 3; names_["o4"] = 4; names_["o5"] = 5; names_["i0"] = 6; names_["i1"] = 7; names_["i2"] = 8; names_["i3"] = 9; names_["i4"] = 10; names_["i5"] = 11; names_["i6"] = 12; names_["i7"] = 13; } /// 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 0x" << 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 << 6 | servoPos_[n]; serial_.putchar (v); log_ (Log::debug) << "send 0x" << 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 << 6 | servoPos_[n]; serial_.putchar (v); log_ (Log::debug) << "send 0x" << std::hex << v << std::dec << std::endl; } /// Met à jour. void GpioServoPp::update (void) { int c; while ((c = serial_.getchar ()) != -1) { log_ (Log::verydebug) << "recv 0x" << std::hex << c << std::dec << std::endl; switch (c & 0x0f) { case 0x0: inputs_ = (inputs_ & 0x0f) | (c & 0xf0); log_ (Log::debug) << "comp " << (c >> 4) << std::endl; break; case 0x7: inputs_ = (inputs_ & 0xf0) | (c >> 4); log_ (Log::debug) << "digi " << (c >> 4) << std::endl; break; case 0x1: case 0x2: case 0x3: case 0x4: case 0x9: case 0xa: case 0xb: case 0xc: { int n = (c & 0x07) - 1; anaInput_[n] = c >> 3; log_ (Log::debug) << "ana " << n << ' ' << (c >> 3) << std::endl; break; } default: log_ (Log::debug) << "other"; break; } } } /// Récupère le nombre d'entrées/sorties. int GpioServoPp::getNbIo (void) { return nbOutputs_ + nbInputs_; } /// Récupère une entrée/sortie par son nom. int GpioServoPp::getByName (const std::string &name) const { Names::const_iterator i = names_.find (name); if (i != names_.end ()) return i->second; else return -1; } /// Récupère la valeure d'une entrée analogique. unsigned int GpioServoPp::getAna (int n) { return anaInput_[n]; } /// Récupère le nombre d'entrées analogiques. int GpioServoPp::getNbAna (void) const { return nbAnaInputs_; }