// 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 #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 tspm, aspm, tsps, asps; int te, ti, ae, ai; int tsc, asc; int cl, cr; int hx, hy, ha, dh; int z0, z1, z2; int px, py, pa; int pl, pr; int n; int sP, sS, sC, sX, sW, sY, sI; bool host; double iu_mm, footing; int seq; bool ack; 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), hx (0), hy (0), ha (0), dh (0), z0 (0), z1 (0), z2 (0), px (0), py (0), pa (0), n (0), seq (1), ack (true) { 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"); tspm = config.get ("asserv.tspm"); aspm = config.get ("asserv.aspm"); tsps = config.get ("asserv.tsps"); asps = config.get ("asserv.asps"); sP = config.get ("asserv.stat_pos"); sS = config.get ("asserv.stat_speed"); sC = config.get ("asserv.stat_count"); sX = config.get ("asserv.stat_postrack"); sW = config.get ("asserv.stat_pwm"); sI = config.get ("asserv.stat_in", 0); host = config.get ("host"); if (host) { sY = config.get ("asserv.stat_simu"); } iu_mm = config.get ("iu_mm"); footing = config.get ("footing"); 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', "bbbbb", 's', tspm, aspm, tsps, asps); proto_.send ('p', "bw", 'f', footing * iu_mm); proto_.send ('P', "b", sP); proto_.send ('S', "b", sS); proto_.send ('C', "b", sC); proto_.send ('X', "b", sX); proto_.send ('W', "b", sW); proto_.send ('I', "b", sI); if (host) proto_.send ('Y', "b", sY); } void receive (char command, const Proto::Frame &frame) { switch (command) { case 'A': ack = true; break; 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 'X': proto_.decode (frame, "DDD", px, py, pa); break; case 'Y': proto_.decode (frame, "WWWW", hx, hy, ha, dh); break; case 'Z': proto_.decode (frame, "BBB", z0, z1, z2); break; case 'I': proto_.decode (frame, "bb", z0, z1); break; case 'W': if (proto_.decode (frame, "WW", pl, pr)) { std::cout << te << ' ' << ti << ' ' << ae << ' ' << ai << ' ' << pl << ' ' << pr << ' ' << tsc << ' ' << asc << ' ' << hx << ' ' << hy << ' ' << ((double) ha / 1024) << ' ' << dh << ' ' << cl << ' ' << cr << ' ' << z0 << ' ' << z1 << ' ' << z2 << ' ' << ack << ' ' << (px / 256) << ' ' << (py / 256) << ' ' << (360.0 * pa / 256 / 256 / 256) << std::endl; te = ti = ae = ai = 0; tsc = asc = 0; cl = cr = 0; hx = hy = ha = dh = 0; z0 = z1 = z2 = 0; px = py = pa = 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 DUR teste la vitesse\n" " p teste la position à vitesse controlée\n" " r teste la position à vitesse controlée\n" " h teste la detection de trous\n" << std::endl; } void find_holes (void) { if (host) { unsigned int i; static const int d[] = { 50, 25, 0, 75 }; for (i = 0; i < sizeof (d) / sizeof (d[0]); i++) { proto_.send ('y', "bwww", 'X', 1050 + d[i], 300, (int) (M_PI_2 * 1024)); proto_.send ('h', "b", seq++); for (n = 0; n < 1000;) { proto_.wait (-1); if (ack) proto_.send ('a', "b", seq); ack = false; } } } else { proto_.send ('h', "b", seq++); for (n = 0; n < 3000;) proto_.wait (-1); } } int main (void) { if (argc_ == 1) { syntax (); return 1; } for (int i = 1; i < argc_; ++i) { int dur; 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", 1000, 0); for (n = 0; n < 400;) proto_.wait (-1); break; case 's': if (++i >= argc_) throw std::runtime_error ("no duration"); dur = atoi (argv_[i]); if (dur == 0) throw std::runtime_error ("bad duration"); proto_.send ('s', "bb", 64, 0); for (n = 0; n < dur;) proto_.wait (-1); proto_.send ('s'); for (n = 0; n < 100;) proto_.wait (-1); break; case 'p': proto_.send ('s', "ddb", (int) (iu_mm * 420), 0, 1); for (n = 0; n < 500;) proto_.wait (-1); break; case 'r': proto_.send ('s', "ddb", 0, (int) (iu_mm * M_PI_2 * 0.5 * footing), 1); for (n = 0; n < 500;) proto_.wait (-1); break; case 'R': proto_.send ('p', "bbb", 's', 96, 12); proto_.send ('s', "ddb", (int) (iu_mm * 512), (int) (iu_mm * -64), 1); for (n = 0; n < 500;) proto_.wait (-1); proto_.send ('p', "bbb", 's', tspm, aspm); break; case 'h': find_holes (); 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; }