// asserv.cc // marvin - programme du robot 2006. {{{ // // Copyright (C) 2006 Nicolas Schodet // // Robot APB Team/Efrei 2006. // 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 "asserv.hh" #include "config/config.hh" #include /// Constructor. Asserv::Asserv (Asserv::Receiver &receiver) : proto_ (*this), receiver_ (receiver) { // Get Config Config &config = Config::getInstance (); intervalCounterStat_ = config.get ("asserv.counter_stat", 0); intervalPos_ = config.get ("asserv.pos_report"); intervalSpeedStat_ = config.get ("asserv.speed_stat", 0); intervalPosStat_ = config.get ("asserv.pos_stat", 0); intervalPwmStat_ = config.get ("asserv.pwm_stat", 0); intervalTimerStat_ = config.get ("asserv.timer_stat", 0); intervalInPort_ = config.get ("asserv.in_port_report"); footing_ = config.get ("asserv.footing"); tAccel_ = config.get ("asserv.t_accel"); aAccel_ = config.get ("asserv.a_accel"); tMaxSpeed_ = config.get ("asserv.t_max_speed"); aMaxSpeed_ = config.get ("asserv.a_max_speed"); tMaxSpeedSlow_ = config.get ("asserv.t_max_speed_slow"); aMaxSpeedSlow_ = config.get ("asserv.a_max_speed_slow"); tkp_ = config.get ("asserv.tkp"); tki_ = config.get ("asserv.tki"); tkd_ = config.get ("asserv.tkd"); akp_ = config.get ("asserv.akp"); aki_ = config.get ("asserv.aki"); akd_ = config.get ("asserv.akd"); esat_ = config.get ("asserv.esat"); isat_ = config.get ("asserv.isat"); lInvertPwm_ = config.get ("asserv.l_invert_pwm"); rInvertPwm_ = config.get ("asserv.r_invert_pwm"); stepPerMm_ = config.get ("asserv.step_per_mm"); tty_ = config.get ("asserv.tty"); proto_.open (tty_); reset (); } /// Essaie de purger la liste d'émission et indique si elle est vide. bool Asserv::sync (void) { return proto_.sync (); } /// Attend que toute les émissions soit terminées. bool Asserv::wait (int timeout/*-1*/) { return proto_.wait (timeout); } /// Récupère le File Descriptor int Asserv::getFd (void) const { return proto_.getFd (); } /// Reset the board and send parameters. void Asserv::reset (void) { proto_.send ('z'); setIntervalCounterStat (intervalCounterStat_); setIntervalPos (intervalPos_); setIntervalSpeedStat (intervalSpeedStat_); setIntervalPosStat (intervalPosStat_); setIntervalPwmStat (intervalPwmStat_); setIntervalTimerStat (intervalTimerStat_); setIntervalInPort (intervalInPort_); setFooting (footing_); setAccel (tAccel_, aAccel_); setMaxSpeed (tMaxSpeed_, aMaxSpeed_, tMaxSpeedSlow_, aMaxSpeedSlow_); setCoef (tkp_, tki_, tkd_, akp_, akd_, akd_, esat_, isat_); setPwmDir (lInvertPwm_, rInvertPwm_); } /// Set PWM. void Asserv::pwm (int l, int r) { proto_.send ('w', "ww", l, r); } /// Position consign offset. void Asserv::offset (double t, double a) { proto_.send ('c', "ww", mmToStep (t), mmToStep (a)); } /// Set speed. void Asserv::speed (int t, int a) { proto_.send ('s', "bb", t, a); } /// Speed controlled position consign offset. void Asserv::speedTo (double t, double a, int seq) { proto_.send ('s', "ddb", mmToStep (t), mmToStep (a), seq); } /// Speed controlled angle consign offset. void Asserv::speedAngle (double a, int seq) { proto_.send ('s', "ddb", 0, radToStep (a), seq); } /// Find a hole. void Asserv::findHole (int seq) { proto_.send ('h', "b", seq); } /// Acknoledge. void Asserv::ack (int seq) { proto_.send ('a', "b", seq); } /// Change counter stat interval. void Asserv::setIntervalCounterStat (int i) { intervalCounterStat_ = i; proto_.send ('C', "b", i); } /// Change position report interval. void Asserv::setIntervalPos (int i) { intervalPos_ = i; proto_.send ('X', "b", i); } /// Change speed stat interval. void Asserv::setIntervalSpeedStat (int i) { intervalSpeedStat_ = i; proto_.send ('S', "b", i); } /// Change position control stat interval. void Asserv::setIntervalPosStat (int i) { intervalPosStat_ = i; proto_.send ('P', "b", i); } /// Change pwm stat interval. void Asserv::setIntervalPwmStat (int i) { intervalPwmStat_ = i; proto_.send ('W', "b", i); } /// Change timer stat interval. void Asserv::setIntervalTimerStat (int i) { intervalTimerStat_ = i; proto_.send ('T', "b", i); } /// Change input port report interval. void Asserv::setIntervalInPort (int i) { intervalInPort_ = i; proto_.send ('I', "b", i); } /// Set current position. void Asserv::setPos (double x, double y, double a) { setXPos (x); setYPos (y); setAngle (a); } /// Set current x position. void Asserv::setXPos (double x) { proto_.send ('p', "bd", 'X', mmToStep (x)); } /// Set current y position. void Asserv::setYPos (double y) { proto_.send ('p', "bd", 'Y', mmToStep (y)); } /// Set current angle. void Asserv::setAngle (double a) { proto_.send ('p', "bd", 'A', radToAvr (a)); } /// Set footing. void Asserv::setFooting (int f) { footing_ = f; proto_.send ('p', "bw", 'f', f); } /// Set acceleration. void Asserv::setAccel (int t, int a) { tAccel_ = t; aAccel_ = a; proto_.send ('p', "bww", 'a', t, a); } /// Set maximum speed for automatic movements. void Asserv::setMaxSpeed (int t, int a, int ts, int as) { tMaxSpeed_ = t; aMaxSpeed_ = a; tMaxSpeedSlow_ = ts; aMaxSpeedSlow_ = as; proto_.send ('p', "bbbbb", 's', t, a, ts, as); } /// Set motor control coeficients. void Asserv::setCoef (int tkp, int tki, int tkd, int akp, int aki, int akd, int esat, int isat) { tkp_ = tkp; tki_ = tki; tkd_ = tkd; akp_ = akp; aki_ = aki; akd_ = akd; esat_ = esat; isat_ = isat; proto_.send ('p', "bww", 'p', tkp, akp); proto_.send ('p', "bww", 'i', tki, aki); proto_.send ('p', "bww", 'd', tkd, akd); proto_.send ('p', "bw", 'E', esat); proto_.send ('p', "bw", 'I', isat); } /// Set PWM direction. void Asserv::setPwmDir (bool invertL, bool invertR) { lInvertPwm_ = invertL; rInvertPwm_ = invertR; proto_.send ('p', "bbb", 'w', invertL ? 1 : 0, invertR ? 1 : 0); } /// Store to eeprom. void Asserv::storeParams (void) { proto_.send ('p', "bb", 'E', 1); } /// Clear eeprom. void Asserv::clearParams (void) { proto_.send ('p', "bb", 'E', 0); } /// Implémentation du proto::Receiver. void Asserv::receive (char command, const Proto::Frame &frame) { int seq, l, r, x, y, a, t, te, ti, ae, ai, port; switch (command) { case 'A': if (proto_.decode (frame, "b", seq)) receiver_.receiveAck (seq); break; case 'C': if (proto_.decode (frame, "ww", l, r)) receiver_.receiveCounterStat (l, r); break; case 'X': if (proto_.decode (frame, "DDD", x, y, a)) receiver_.receivePos (stepToMm (x / 256), stepToMm (y / 256), avrToRad (a)); break; case 'S': if (proto_.decode (frame, "BB", t, a)) receiver_.receiveSpeedStat (t, a); break; case 'P': if (proto_.decode (frame, "WWWW", te, ti, ae, ai)) receiver_.receivePosStat (te, ti, ae, ai); break; case 'W': if (proto_.decode (frame, "WW", l, r)) receiver_.receivePwmStat (l, r); break; case 'T': // TODO (si on a le temps). break; case 'I': if (proto_.decode (frame, "w", port)) receiver_.receiveInPort (port); break; } } /// Convert mm to steps. int Asserv::mmToStep (double mm) const { return static_cast (mm * stepPerMm_); } /// Convert steps to mm. double Asserv::stepToMm (int step) const { return static_cast (step) / stepPerMm_; } /// Convert rad to avr angles. int Asserv::radToAvr (double a) const { return static_cast (a * M_1_PI * 0.5 * (1 << 24)) & 0xffffff; } /// Convert avr angles to rad. double Asserv::avrToRad (int a) const { return static_cast (a) * 2 * M_PI / (1 << 24); } /// Convert rad to steps. int Asserv::radToStep (double a) const { return static_cast (static_cast (footing_) * a * 0.5); }