summaryrefslogtreecommitdiff
path: root/i/marvin/src/asserv/test_asserv.cc
diff options
context:
space:
mode:
authorschodet2006-05-21 00:27:58 +0000
committerschodet2006-05-21 00:27:58 +0000
commita0375158e9dda56c60fe37318d320ac1e54a2049 (patch)
tree5d17f790ed66108f8d77169865c819f9eaede255 /i/marvin/src/asserv/test_asserv.cc
parentcd5fd2efaa1867932daf467ed7a64f4928e3578a (diff)
Renomage de fonctions, ajout du test et non commit d'un fichier .old :).
Diffstat (limited to 'i/marvin/src/asserv/test_asserv.cc')
-rw-r--r--i/marvin/src/asserv/test_asserv.cc452
1 files changed, 138 insertions, 314 deletions
diff --git a/i/marvin/src/asserv/test_asserv.cc b/i/marvin/src/asserv/test_asserv.cc
index 7cd2f95..4742e5d 100644
--- a/i/marvin/src/asserv/test_asserv.cc
+++ b/i/marvin/src/asserv/test_asserv.cc
@@ -1,7 +1,7 @@
// test_asserv.cc
-// robert - programme du robot 2005 {{{
+// marvin - programme du robot 2006. {{{
//
-// Copyright (C) 2005 Nicolas Haller
+// Copyright (C) 2006 Nicolas Schodet
//
// Robot APB Team/Efrei 2005.
// Web: http://assos.efrei.fr/robot/
@@ -22,327 +22,151 @@
// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
//
// }}}
-
#include "asserv.hh"
-#include "config/config.hh"
-#include "timer/timer.hh"
+#include "tester/tester.hh"
-#include<stdexcept>
-#include<cstdlib>
-#include<cmath>
+#include <iostream>
+#include <exception>
+#include <iomanip>
-/// Classe de test pour asserv.
-class TestAsserv : public Asserv::Receiver
+class TestAsserv : public Tester, 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;
- }
-
+ private:
+ Asserv asserv_;
+ int seq_;
+ bool ok_;
+ public:
+ /// Constructor.
+ TestAsserv (int argc, char **argv)
+ : Tester (argc, argv), asserv_ (*this), seq_ (0), ok_ (true)
+ { }
+ /// Next seq number.
+ inline void nextSeq (void)
+ {
+ seq_++;
+ if (seq_ > 250)
+ seq_ = 1;
+ }
+ /// Speed controlled position consign offset, handle seq.
+ void speedTo (double t, double a)
+ {
+ nextSeq ();
+ ok_ = false;
+ asserv_.speedTo (t, a, seq_);
+ }
+ /// Find a hole, handle seq.
+ void findHole (void)
+ {
+ nextSeq ();
+ ok_ = false;
+ asserv_.findHole (seq_);
+ }
+ /// Called after each command called.
+ void postcall (void)
+ {
+ while (!asserv_.wait () || !ok_)
+ std::cout << "hop" << std::endl;
+ }
+ /// Executed before checking/running commands. Good place to add command
+ /// to the interpreter.
+ void preRun (void)
+ {
+ Interpreter &i = getInterpreter ();
+ i.add ("wait", Interpreter::memFunc (asserv_, &Asserv::wait),
+ "wait MS\nwait for a delay in millisecond");
+ i.add ("reset", Interpreter::memFunc (asserv_, &Asserv::reset),
+ "reset\nreset the asserv board");
+ i.add ("pwm", Interpreter::memFunc (asserv_, &Asserv::pwm),
+ "pwm LEFT RIGHT\n"
+ "directly set pwm values, use with caution");
+ i.add ("offset", Interpreter::memFunc (asserv_, &Asserv::offset),
+ "offset THETA(mm) ALPHA(mm)\n"
+ "add value to current position, use for pid callibration");
+ i.add ("speed", Interpreter::memFunc (asserv_, &Asserv::speed),
+ "speed THETA ALPHA\n"
+ "unlimited speed consign");
+ i.add ("speedTo", Interpreter::memFunc (*this, &TestAsserv::speedTo),
+ "speedTo THETA(mm) ANGLE(mm)\n"
+ "speed consign limited in distance");
+ i.add ("findHole", Interpreter::memFunc (*this, &TestAsserv::findHole),
+ "findHole\nfind a hole");
+ i.add ("setPos", Interpreter::memFunc (asserv_, &Asserv::setPos),
+ "setPos X(mm) Y(mm) A(rad)\n"
+ "set current position");
+ i.add ("setXPos", Interpreter::memFunc (asserv_, &Asserv::setYPos),
+ "setPos X(mm)\n"
+ "set current X position");
+ i.add ("setYPos", Interpreter::memFunc (asserv_, &Asserv::setXPos),
+ "setPos Y(mm)\n"
+ "set current Y position");
+ i.add ("setAngle", Interpreter::memFunc (asserv_, &Asserv::setAngle),
+ "setPos A(rad)\n"
+ "set current angle");
+ i.add ("storeParams",
+ Interpreter::memFunc (asserv_, &Asserv::storeParams),
+ "storeParams\nstore parameters to eeprom");
+ i.add ("clearParams",
+ Interpreter::memFunc (asserv_, &Asserv::clearParams),
+ "clearParams\nclear parameters stored in eeprom");
+ i.add ("_postcall",
+ Interpreter::memFunc (*this, &TestAsserv::postcall));
+ }
+ /// Executed after running commands.
+ void postRun (void)
+ {
+ asserv_.reset ();
+ }
+ void receiveAck (int seq)
+ {
+ if (seq != seq_)
+ std::cout << "spurious ";
+ std::cout << "ack received" << std::endl;
+ asserv_.ack (seq);
+ ok_ = true;
+ }
+ void receiveCounterStat (int l, int r)
+ {
+ std::cout << "C " << l << ' ' << r << std::endl;
+ }
+ void receivePos (double x, double y, double a)
+ {
+ std::cout << "X " << x << ' ' << y << ' ' << a << std::endl;
+ }
+ void receiveSpeedStat (int t, int a)
+ {
+ std::cout << "S " << t << ' ' << a << std::endl;
+ }
+ void receivePosStat (int te, int ti, int ae, int ai)
+ {
+ std::cout << "P " << te << ' ' << ti << ' ' << ae << ' ' << ai <<
+ std::endl;
+ }
+ void receivePwmStat (int l, int r)
+ {
+ std::cout << "W " << l << ' ' << r << std::endl;
+ }
+ void receiveTimerStat (const int *t, int tn)
+ {
+ std::cout << "T ";
+ std::copy (t, t + tn, std::ostream_iterator<int> (std::cout, " "));
+ }
+ void receiveInPort (unsigned int port)
+ {
+ std::cout << "I " << std::hex << std::setw (4) << std::setfill ('0')
+ << port << std::dec << 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 <cmd> <args...> envois une commande\n"
- " w <ms> attend pendant un nombre de millisecondes\n"
- " ? affiche cet écran d'aide\n"
- << std::endl;
-}
-
- int
+int
main (int argc, char **argv)
{
try
- {
- int i;
- if (argc < 2)
- {
- syntax ();
- return 1;
- }
- Config config(argc, argv);
- TestAsserv testAsserv;
- Asserv asserv (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 ())
- ;
- 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++;
- }
- }
+ {
+ TestAsserv ta (argc, argv);
+ ta.run ();
+ }
catch (const std::exception &e)
- {
+ {
std::cerr << e.what () << std::endl;
- syntax ();
- return 1;
- }
- return 0;
+ }
}