From 21af4d885eff12ebe6c6d0472ec4b3897bcbaeae Mon Sep 17 00:00:00 2001 From: haller Date: Sat, 16 Apr 2005 23:43:45 +0000 Subject: Modifications diverses Codages de test_asserv --- 2005/i/robert/src/asserv/test_asserv.cc | 277 ++++++++++++++++++++++++++++++++ 1 file changed, 277 insertions(+) create mode 100644 2005/i/robert/src/asserv/test_asserv.cc (limited to '2005/i/robert/src/asserv/test_asserv.cc') diff --git a/2005/i/robert/src/asserv/test_asserv.cc b/2005/i/robert/src/asserv/test_asserv.cc new file mode 100644 index 0000000..d7c8964 --- /dev/null +++ b/2005/i/robert/src/asserv/test_asserv.cc @@ -0,0 +1,277 @@ +#ifndef test_asserv_hh +#define test_asserv_hh +// asserv.hh +// 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. +// +// }}} + +/// 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:" << "aPos:" << aPos << std::endl; + } + void receiveMotor (int oldLSpeed_e, int LSpeed_int, int oldRSpeed_e, int RSpeed_int) + { + std::cout << "S:" << "oldLSpeed_e:" << oldLSpeed_e << "LSpeed_int:" << + LSpeed_int << "oldRSpeed_e:" << oldRSpeed_e << + "RSpeed_int:" << RSpeed_int << std::endl; + } + void receivePwm (double leftPwm, double rightPwm) + { + std::cout << "W:" << "leftPwm" << leftPwm << " rightPwm:" << rightPwm << std::endl; + } + void receiveTiming (double motorTimer5, double motorTimer4, + double motorTimer3, double motorTimer2, + double motorTimer1, + double motorTimer0 ) + { + std::cout << "T:" << "motorTimer5:" << motorTimer5 << + " motorTimer4:" << motorTimer4 << "motorTimer3:" << + motorTimer3 << "motorTimer2:" << motorTimer2 << + "motorTimer1:" << motorTimer1 << "motorTimer0:" << + motorTimer0 << std::endl; + } + void receiveInPort (double state) + { + std::cout << "P:" << "state:" << state << std::endl; + } + void jaifini (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; + } + TestAsserv testAsserv; + Asserv asserv(config, testAsserv); + //proto.open (argv[1]); + // XXX Charger la conf + i = 2; + while (i < argc) + { + switch (argv[i][0]) + { + case 's': + { + switch(arg[++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], 0)); + break; + case 'f': + if(++i > argc) + throw std::runtime_error("syntax error"); + asserv.fuckTheWall(strtod(argv[i], 0)); + break; + case 's': + if(++i + 1 > argc) + throw std::runtime_error("syntax error"); + asserv.setSpeed(strtod(argv[i], 0), strtod(argv[++i], 0)); + break; + case 'w': + if(++i + 1 > argc) + throw std::runtime_error("syntax error"); + asserv.setPwm(strtod(argv[i], 0), strtod(argv[++i], 0)); + break; + case 'F': + asserv.finishAck(); + break; + case 'C': + if(++i > argc) + throw std::runtime_error("syntax error"); + asserv.statCounter(strtod(argv[i], 0)); + break; + case 'X': + if(++i > argc) + throw std::runtime_error("syntax error"); + asserv.statPosition(strtod(argv[i], 0)); + break; + case 'S': + if(++i > argc) + throw std::runtime_error("syntax error"); + asserv.statMotor(strtod(argv[i], 0)); + break; + case 'W': + if(++i > argc) + throw std::runtime_error("syntax error"); + asserv.statPwm(strtod(argv[i], 0)); + break; + case 'T': + if(++i > argc) + throw std::runtime_error("syntax error"); + asserv.statTiming(strtod(argv[i], 0)); + break; + case 'P': + if(++i > argc) + throw std::runtime_error("syntax error"); + asserv.fuckTheWall(strtod(argv[i], 0)); + 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(strtod(argv[i], 0)); + 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(strtod(argv[i], 0)); + break; + case 'i': + if(++i > argc) + throw std::runtime_error("syntax error"); + asserv.setKi(strtod(argv[i], 0)); + break; + case 'd': + if(++i > argc) + throw std::runtime_error("syntax error"); + asserv.setKd(strtod(argv[i], 0)); + break; + case 'I': + if(++i > argc) + throw std::runtime_error("syntax error"); + asserv.setSpeedIntMax(strtoi(argv[i], 0)); + break; + case 'a': + if(++i > argc) + throw std::runtime_error("syntax error"); + asserv.setAccel(strtod(argv[i], 0)); + break; + case 'm': + if(++i > argc) + throw std::runtime_error("syntax error"); + asserv.setMaxSpeed(strtod(argv[i], 0)); + 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; +} + +#endif // test_asserv.hh -- cgit v1.2.3