From a0375158e9dda56c60fe37318d320ac1e54a2049 Mon Sep 17 00:00:00 2001 From: schodet Date: Sun, 21 May 2006 00:27:58 +0000 Subject: Renomage de fonctions, ajout du test et non commit d'un fichier .old :). --- i/marvin/src/asserv/Makefile.defs | 4 +- i/marvin/src/asserv/asserv.cc | 27 ++- i/marvin/src/asserv/asserv.hh | 14 +- i/marvin/src/asserv/test_asserv.cc | 452 +++++++++++-------------------------- 4 files changed, 163 insertions(+), 334 deletions(-) (limited to 'i') diff --git a/i/marvin/src/asserv/Makefile.defs b/i/marvin/src/asserv/Makefile.defs index c56d34c..22ffdb2 100644 --- a/i/marvin/src/asserv/Makefile.defs +++ b/i/marvin/src/asserv/Makefile.defs @@ -1,5 +1,5 @@ PROGRAMS += test_asserv -asserv_OBJECTS = asserv.o +asserv_OBJECTS = asserv.o $(proto_OBJECTS) -test_asserv_OBJECTS = test_asserv.o $(asserv_OBJECTS) $(config_OBJECTS) $(timer_OBJECTS) $(proto_OBJECTS) $(log_OBJECTS) $(serial_OBJECTS) $(utils_OBJECTS) +test_asserv_OBJECTS = test_asserv.o $(asserv_OBJECTS) $(tester_OBJECTS) diff --git a/i/marvin/src/asserv/asserv.cc b/i/marvin/src/asserv/asserv.cc index e4e964e..f5321fc 100644 --- a/i/marvin/src/asserv/asserv.cc +++ b/i/marvin/src/asserv/asserv.cc @@ -43,6 +43,8 @@ Asserv::Asserv (Asserv::Receiver &receiver) aAccel_ = config.get ("asserv.a_accel"); tMaxSpeed_ = config.get ("asserv.t_max_speed"); aMaxSpeed_ = config.get ("asserv.a_max_speed"); + tMaxSpeedSlow_ = config.get ("asserv.t_max_speed_slow"); + aMaxSpeedSlow_ = config.get ("asserv.a_max_speed_slow"); tkp_ = config.get ("asserv.tkp"); tki_ = config.get ("asserv.tki"); tkd_ = config.get ("asserv.tkd"); @@ -56,6 +58,7 @@ Asserv::Asserv (Asserv::Receiver &receiver) stepPerMm_ = config.get ("asserv.step_per_mm"); tty_ = config.get ("asserv.tty"); proto_.open (tty_); + reset (); } /// Essaie de purger la liste d'émission et indique si elle est vide. @@ -93,35 +96,35 @@ Asserv::reset (void) setIntervalInPort (intervalInPort_); setFooting (footing_); setAccel (tAccel_, aAccel_); - setMaxSpeed (tMaxSpeed_, aMaxSpeed_); + setMaxSpeed (tMaxSpeed_, aMaxSpeed_, tMaxSpeedSlow_, aMaxSpeedSlow_); setCoef (tkp_, tki_, tkd_, akp_, akd_, akd_, esat_, isat_); setPwmDir (lInvertPwm_, rInvertPwm_); } /// Set PWM. void -Asserv::setPwm (int l, int r) +Asserv::pwm (int l, int r) { proto_.send ('w', "ww", l, r); } /// Position consign offset. void -Asserv::positionOffset (double t, double a) +Asserv::offset (double t, double a) { proto_.send ('c', "ww", mmToStep (t), mmToStep (a)); } /// Set speed. void -Asserv::setSpeed (int t, int a) +Asserv::speed (int t, int a) { proto_.send ('s', "bb", t, a); } /// Speed controlled position consign offset. void -Asserv::setSpeedControlled (double t, double a, int seq) +Asserv::speedTo (double t, double a, int seq) { proto_.send ('s', "ddb", mmToStep (t), mmToStep (a), seq); } @@ -137,7 +140,7 @@ Asserv::findHole (int seq) void Asserv::ack (int seq) { - proto_.send ('A', "b", seq); + proto_.send ('a', "b", seq); } /// Change counter stat interval. @@ -202,7 +205,7 @@ Asserv::setPos (double x, double y, double a) { setXPos (x); setYPos (y); - setAPos (a); + setAngle (a); } /// Set current x position. @@ -221,7 +224,7 @@ Asserv::setYPos (double y) /// Set current angle. void -Asserv::setAPos (double a) +Asserv::setAngle (double a) { proto_.send ('p', "bd", 'A', radToAvr (a)); } @@ -245,11 +248,13 @@ Asserv::setAccel (int t, int a) /// Set maximum speed for automatic movements. void -Asserv::setMaxSpeed (int t, int a) +Asserv::setMaxSpeed (int t, int a, int ts, int as) { tMaxSpeed_ = t; aMaxSpeed_ = a; - proto_.send ('p', "bbb", 's', t, a); + tMaxSpeedSlow_ = ts; + aMaxSpeedSlow_ = as; + proto_.send ('p', "bbbbb", 's', t, a, ts, as); } /// Set motor control coeficients. @@ -312,7 +317,7 @@ Asserv::receive (char command, const Proto::Frame &frame) break; case 'X': if (proto_.decode (frame, "DDD", x, y, a)) - receiver_.receivePos (stepToMm (x), stepToMm (y), stepToMm (a)); + receiver_.receivePos (stepToMm (x), stepToMm (y), avrToRad (a)); break; case 'S': if (proto_.decode (frame, "BB", t, a)) diff --git a/i/marvin/src/asserv/asserv.hh b/i/marvin/src/asserv/asserv.hh index 86cbb23..5f59084 100644 --- a/i/marvin/src/asserv/asserv.hh +++ b/i/marvin/src/asserv/asserv.hh @@ -57,7 +57,7 @@ class Asserv : public Proto::Receiver intervalInPort_; int footing_; int tAccel_, aAccel_; - int tMaxSpeed_, aMaxSpeed_; + int tMaxSpeed_, aMaxSpeed_, tMaxSpeedSlow_, aMaxSpeedSlow_; int tkp_, tki_, tkd_, akp_, aki_, akd_, esat_, isat_; bool lInvertPwm_, rInvertPwm_; double stepPerMm_; @@ -73,13 +73,13 @@ class Asserv : public Proto::Receiver /// Reset the board and send parameters. void reset (void); /// Set PWM. - void setPwm (int l, int r); + void pwm (int l, int r); /// Position consign offset. - void positionOffset (double t, double a); + void offset (double t, double a); /// Set speed. - void setSpeed (int t, int a); + void speed (int t, int a); /// Speed controlled position consign offset. - void setSpeedControlled (double t, double a, int seq); + void speedTo (double t, double a, int seq); /// Find a hole. void findHole (int seq); /// Acknoledge. @@ -105,13 +105,13 @@ class Asserv : public Proto::Receiver /// Set current y position. void setYPos (double y); /// Set current angle. - void setAPos (double a); + void setAngle (double a); /// Set footing. void setFooting (int f); /// Set acceleration. void setAccel (int t, int a); /// Set maximum speed for automatic movements. - void setMaxSpeed (int t, int a); + void setMaxSpeed (int t, int a, int ts, int as); /// Set motor control coeficients. void setCoef (int tkp, int tki, int tkd, int akp, int aki, int akd, int esat, int isat); 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 -#include -#include +#include +#include +#include -/// 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 (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 envois une commande\n" - " w 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; + } } -- cgit v1.2.3