// asserv_graph.cc // {{{ // // 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 "proto/proto.hh" #include "timer/timer.hh" #include "config/config.hh" #include #include /// Classe de connexion à Proto. class AsservGraph : public Proto::Receiver { Proto proto_; int argc_; char **argv_; int tkp, tki, tkd; int akp, aki, akd; int es, is; int ta, aa; int te, ti, ae, ai; int tsc, asc; int cl, cr; int pl, pr; int n; public: AsservGraph (int argc, char **argv) : proto_ (*this), argc_ (argc), argv_ (argv), te (0), ti (0), ae (0), ai (0), tsc (0), asc (0), cl (0), cr (0), n (0) { Config &config = Config::getInstance (); 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"); es = config.get ("asserv.esat"); is = config.get ("asserv.isat"); ta = config.get ("asserv.ta"); aa = config.get ("asserv.aa"); proto_.open (config.get ("asserv.tty")); init (); } ~AsservGraph (void) { proto_.send ('z'); } void init (void) { proto_.send ('z'); 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', es); proto_.send ('p', "bw", 'I', is); proto_.send ('p', "bww", 'a', ta, aa); proto_.send ('P', "b", 1); proto_.send ('S', "b", 1); proto_.send ('C', "b", 1); proto_.send ('W', "b", 1); } void receive (char command, const Proto::Frame &frame) { switch (command) { case 'P': proto_.decode (frame, "WWWW", te, ti, ae, ai); break; case 'S': proto_.decode (frame, "BB", tsc, asc); break; case 'C': proto_.decode (frame, "WW", cl, cr); break; case 'W': if (proto_.decode (frame, "WW", pl, pr)) { std::cout << te << ' ' << ti << ' ' << ae << ' ' << ai << ' ' << pl << ' ' << pr << ' ' << tsc << ' ' << asc << ' ' << cl << ' ' << cr << std::endl; te = ti = ae = ai = 0; tsc = asc = 0; cl = cr = 0; n++; } break; } } void syntax (void) { std::cout << "asserv_graph - graphe des variables de" " l'asservissement.\n" "Syntaxe : asserv_graph [commands]\n" " w teste la pwm\n" " c teste le pas\n" " s teste la vitesse\n" " p teste la position à vitesse controlée\n" << std::endl; } int main (void) { if (argc_ == 1) { syntax (); return 1; } for (int i = 1; i < argc_; ++i) { if (argv_[i][0] == '\0' || argv_[i][1] != '\0') throw std::runtime_error ("bad command line"); switch (argv_[i][0]) { case 'w': proto_.send ('w', "ww", 0x3ff, 0x3ff); for (n = 0; n < 100;) proto_.wait (-1); break; case 'c': proto_.send ('c', "ww", 64, 0); for (n = 0; n < 100;) proto_.wait (-1); break; case 's': proto_.send ('s', "bb", 64, 0); for (n = 0; n < 100;) proto_.wait (-1); proto_.send ('s'); for (n = 0; n < 100;) proto_.wait (-1); break; case 'p': proto_.send ('s', "wwbb", 1024, 0, 64, 0); for (n = 0; n < 100;) proto_.wait (-1); break; default: throw std::runtime_error ("bad command line"); break; } proto_.send ('w'); for (n = 0; n < 10;) proto_.wait (-1); } return 0; } }; int main (int argc, char **argv) { try { Config config (argc, argv); AsservGraph asservGraph (argc, argv); return asservGraph.main (); } catch (const std::exception &e) { std::cerr << e.what () << std::endl; return 1; } return 0; }