// test_asserv.cc // robert - programme du robot 2005 {{{ // // Copyright (C) 2005 Nicolas Haller // // Robot APB Team/Efrei 2005. // 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 "timer/timer.hh" #include #include #include /// Classe de test pour asserv. class TestAsserv : public Asserv::Receiver { void receiveCounter (double lMotor, double rMotor) { std::cout << "C:" << "lMotor:" << lMotor << " rMotor:" << rMotor << std::endl; } void receivePosX (double xPos) { std::cout << "X:" << "XPos:" << xPos << std::endl; } void receivePosY (double yPos) { std::cout << "Y:" << "yPos:" << yPos << std::endl; } void receivePosA (double aPos) { std::cout << "A:" << " aPosR: " << aPos << " aPosD: " << aPos / (2 * M_PI) * 360 << std::endl; } void receiveSpeedStat (int leftError, int leftInt, int rightError, int rightInt) { std::cout << "S:" << " leftError: " << leftError << "leftInt: " << leftInt << " rightError: " << rightError << " rightInt: " << rightInt << std::endl; } void receivePwm (double leftPwm, double rightPwm) { std::cout << "W:" << "leftPwm" << leftPwm << " rightPwm:" << rightPwm << std::endl; } void receiveTiming (int motorTimer4, int motorTimer3, int motorTimer2, int motorTimer1, int motorTimer0 ) { std::cout << "T:" << " motorTimer4: " << motorTimer4 << " motorTimer3: " << motorTimer3 << " motorTimer2: " << motorTimer2 << " motorTimer1: " << motorTimer1 << " motorTimer0: " << motorTimer0 << std::endl; } void receiveInPort (int port) { std::cout << "P:" << "state:" << port << std::endl; } void receiveSharp (int sharp0, int sharp1, int sharp2) { std::cout << "t:" << "sharp0:" << sharp0 << " sharp1:" << sharp1 << " sharp2:" << sharp2 << std::endl; } void done (void) { std::cout << "F:" << "J'ai fini!!" << std::endl; } }; /// Affiche un memo de suntaxe. void syntax (void) { std::cout << "test_asserv - test la classe de protocol asserv.\n" "Syntaxe : test_asserv <...>\n" " s envois une commande\n" " w attend pendant un nombre de millisecondes\n" " ? affiche cet écran d'aide\n" << std::endl; } int main (int argc, char **argv) { try { int i; if (argc < 2) { syntax (); return 1; } Config config(argc, argv); TestAsserv testAsserv; Asserv asserv(config, testAsserv); i = 1; while (i < argc) { switch (argv[i][0]) { case 's': { switch(argv[++i][0]) { case 'z': asserv.reset(); break; case 'l': if(++i > argc) throw std::runtime_error("syntax error"); asserv.linearMove(strtod(argv[i], 0)); break; case 'a': if(++i > argc) throw std::runtime_error("syntax error"); asserv.angularMove(strtod(argv[i], 0)); break; case 'g': if(++i + 1 > argc) throw std::runtime_error("syntax error"); asserv.goToPosition(strtod(argv[i], 0), strtod(argv[i + 1], 0)); i++; break; case 'f': if(++i + 1 > argc) throw std::runtime_error("syntax error"); switch(argv[i][0]) { case 'd': asserv.fuckTheWall(strtod(argv[i + 1], 0)); break; case 'i': asserv.fuckTheWall(atoi(argv[i + 1])); break; default: throw std::runtime_error("syntax error"); break; } i++; break; case 's': if(++i + 2 > argc) throw std::runtime_error("syntax error"); switch(argv[i][0]) { case 'd': i++; asserv.setSpeed(strtod(argv[i], 0), strtod(argv[i + 1], 0)); break; case 'i': i++; asserv.setSpeed(atoi(argv[i]), atoi(argv[i + 1])); break; default: throw std::runtime_error("syntax error"); break; } i++; break; case 'w': if(++i + 1 > argc) throw std::runtime_error("syntax error"); asserv.setPwm(strtol(argv[i], 0, 10), strtol(argv[i + 1], 0, 10)); i++; break; case 'F': asserv.finishAck(); break; case 'C': if(++i > argc) throw std::runtime_error("syntax error"); asserv.statCounter(strtol(argv[i], 0, 10)); break; case 'X': if(++i > argc) throw std::runtime_error("syntax error"); asserv.statPosition(strtol(argv[i], 0, 10)); break; case 'S': if(++i > argc) throw std::runtime_error("syntax error"); asserv.statMotor(strtol(argv[i], 0, 10)); break; case 'W': if(++i > argc) throw std::runtime_error("syntax error"); asserv.statPwm(strtol(argv[i], 0, 10)); break; case 'T': if(++i > argc) throw std::runtime_error("syntax error"); asserv.statTiming(strtol(argv[i], 0, 10)); break; case 'P': if(++i > argc) throw std::runtime_error("syntax error"); asserv.statInPort(strtol(argv[i], 0, 10)); break; case 'H': if(++i > argc) throw std::runtime_error("syntax error"); asserv.statSharp(atoi(argv[i])); break; case 'p': if(++i > argc) throw std::runtime_error("syntax error"); switch(argv[i][0]) { case 'x': if(++i > argc) throw std::runtime_error("syntax error"); asserv.setXPos(strtod(argv[i], 0)); break; case 'y': if(++i > argc) throw std::runtime_error("syntax error"); asserv.setYPos(strtod(argv[i], 0)); break; case 'a': if(++i > argc) throw std::runtime_error("syntax error"); asserv.setAngle(strtod(argv[i], 0)); break; case 'f': if(++i > argc) throw std::runtime_error("syntax error"); asserv.setFooting(strtol(argv[i], 0, 10)); break; case 'e': if(++i > argc) throw std::runtime_error("syntax error"); asserv.setEpsilon(strtod(argv[i], 0)); break; case 'p': if(++i > argc) throw std::runtime_error("syntax error"); asserv.setKp(strtol(argv[i], 0, 10)); break; case 'i': if(++i > argc) throw std::runtime_error("syntax error"); asserv.setKi(strtol(argv[i], 0, 10)); break; case 'd': if(++i > argc) throw std::runtime_error("syntax error"); asserv.setKd(strtol(argv[i], 0, 10)); break; case 'E': if(++i > argc) throw std::runtime_error("syntax error"); asserv.setESat(strtol(argv[i], 0, 10)); break; case 'I': if(++i > argc) throw std::runtime_error("syntax error"); asserv.setSpeedIntMax(strtol(argv[i], 0, 10)); break; case 's': if(++i > argc) throw std::runtime_error("syntax error"); asserv.setDSample(strtol(argv[i], 0, 10)); break; case 'A': if(++i > argc) throw std::runtime_error("syntax error"); asserv.setAccel(strtol(argv[i], 0, 10)); break; case 'm': if(++i > argc) throw std::runtime_error("syntax error"); asserv.setMaxSpeed(strtol(argv[i], 0, 10), strtol(argv[i + 1], 0, 10)); i++; break; case 'W': if (++i + 1 > argc) throw std::runtime_error("syntax error"); asserv.setInvPwm(atoi(argv[i]), atoi(argv[i + 1])); i++; break; } } while (!asserv.wait (-1)); break; } case 'w': { int stop, t; if (i + 1 >= argc) throw std::runtime_error ("syntax error"); stop = atoi (argv[++i]) + Timer::getProgramTime (); t = Timer::getProgramTime (); while (t < stop) { asserv.wait (stop - t); t = Timer::getProgramTime (); } break; } case '?': syntax (); return 0; default: throw std::runtime_error ("syntax error"); } i++; } } catch (const std::exception &e) { std::cerr << e.what () << std::endl; syntax (); return 1; } return 0; }