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/asserv.cc | 96 ++++++++++- 2005/i/robert/src/asserv/asserv.hh | 27 ++-- 2005/i/robert/src/asserv/test_asserv.cc | 277 ++++++++++++++++++++++++++++++++ 3 files changed, 384 insertions(+), 16 deletions(-) create mode 100644 2005/i/robert/src/asserv/test_asserv.cc (limited to '2005/i/robert') diff --git a/2005/i/robert/src/asserv/asserv.cc b/2005/i/robert/src/asserv/asserv.cc index ed24fe6..e3a0f59 100644 --- a/2005/i/robert/src/asserv/asserv.cc +++ b/2005/i/robert/src/asserv/asserv.cc @@ -29,10 +29,10 @@ /// Constructeur Asserv::Asserv (const Config & config, Receiver & receiver) - :proto_(*this), log_("asserv") + :proto_(*this), receiver_(receiver), log_("asserv") { // Rechargement des paramètres - loadConfig(); + loadConfig(config); //Ouverture du port série proto_.open(ttyName_); //Initialisation de l'AVR @@ -109,7 +109,6 @@ void Asserv::setSpeed(double xSpeed, double ySpeed) proto_.send('s',"bb", vx, vy); } -// XXX changer rCycl2Pwm pour prendre en compte le format void Asserv::setPwm(double xPwm, double yPwm) { // Conversion rCycl->Pwm @@ -119,6 +118,12 @@ void Asserv::setPwm(double xPwm, double yPwm) proto_.send('w',"ww" ,xp, yp); } +/// Informe l'AVR d'arréter de dire "j'ai fini" +void Asserv::finishAck(void) +{ + proto_.send('F', "b", 0); +} + /// Statistiques void Asserv::statCounter(double period) { @@ -234,6 +239,11 @@ void Asserv::setKd(double Kd) proto_.send('p',"bw", 'd', static_cast(Kd * 256)); } +void Asserv::setSpeedIntMax(int16_t maxInt) +{ + proto_.send('p', "bW", 'a', maxInt); +} + void Asserv::setAccel(double accel) { // Conversion mmps2ppperiod @@ -253,13 +263,85 @@ void Asserv::setMaxSpeed(double maxSpeed) /// implémentation du proto::Receiver void Asserv::receive(char command, const Proto::Frame &frame) { - // XXX Faire des truc ici + switch(command) + { + case 'C': //Compteurs des moteurs (uint16) + int lMotor; + int rMotor; + proto_.decode(frame, "ww", lMotor, rMotor); + receiver_.receiveCounter(lMotor, rMotor); + break; + case 'X': + int xPos; + proto_.decode(frame, "D", xPos); + // Convertion Pas->mm + xPos *= static_cast(mmPpas_); + receiver_.receivePosX(xPos); + break; + case 'Y': + int yPos; + proto_.decode(frame, "D", yPos); + // Convertion Pas->mm + yPos *= static_cast(mmPpas_); + receiver_.receivePosY(yPos); + break; + case 'A': + int aPos; + proto_.decode(frame, "D", aPos); + aPos = static_cast(aPos * (2 * M_PI)); + aPos /= (1 << 24); + receiver_.receivePosA(aPos); + break; + case 'S': + int oldLSpeed, LSpeed, oldRSpeed, RSpeed; + proto_.decode(frame, "WWWW", oldLSpeed, LSpeed, oldRSpeed, RSpeed); + receiver_.receiveMotor(oldLSpeed, LSpeed, oldRSpeed, RSpeed); + break; + case 'W': + int leftPwm, rightPwm; + proto_.decode(frame, "WW", leftPwm, rightPwm); + receiver_.receivePwm(leftPwm / pwmMax_, rightPwm / pwmMax_); + break; + case 'T': + { + int motorTimer5 = static_cast(frame.args[0]); + int motorTimer4 = static_cast(frame.args[1]); + int motorTimer3 = static_cast(frame.args[2]); + int motorTimer2 = static_cast(frame.args[3]); + int motorTimer1 = static_cast(frame.args[4]); + int motorTimer0 = static_cast(frame.args[5]); + receiver_.receiveTiming(motorTimer5 * sPperiod_, + motorTimer4 * sPperiod_, + motorTimer3 * sPperiod_, + motorTimer2 * sPperiod_, + motorTimer1 * sPperiod_, + motorTimer0 * sPperiod_); + break; + } + case 'P': + { + int statePIn; + proto_.decode(frame, "b", statePIn); + receiver_.receiveInPort(statePIn); + break; + } + case 'F': + receiver_.jaifini(); + break; + } } /// Charge les données de la classe config -void Asserv::loadConfig(void) -{ - // XXX Faire des truc ici +void Asserv::loadConfig(const Config & config) +{ + footing_ = config.get("asserv.footing"); + epsilon_ = config.get("asserv.epsilon"); + accel_ = config.get("asserv.accel"); + kp_ = config.get("asserv.kp"); + ki_ = config.get("asserv.ki"); + kd_ = config.get("asserv.kd"); + maxSpeed_ = config.get("asserv.maxSpeed"); + pwmMax_ = config.get("asserv.maxPwm"); } int Asserv::mm2pasD(double dist) diff --git a/2005/i/robert/src/asserv/asserv.hh b/2005/i/robert/src/asserv/asserv.hh index 83c9f08..ef6a715 100644 --- a/2005/i/robert/src/asserv/asserv.hh +++ b/2005/i/robert/src/asserv/asserv.hh @@ -38,17 +38,25 @@ class Asserv : public NonCopyable, public Proto::Receiver { public: /// Recoit un packet. - virtual void receiveCounter (char command) = 0; - virtual void receivePosition (char command) = 0; - virtual void receiveMotor (char command) = 0; - virtual void receivePwm (char command) = 0; - virtual void receiveTiming (char command) = 0; - virtual void receiveInPort (char command) = 0; - virtual void jaifini (char command) = 0; + virtual void receiveCounter (double lMotor, double rMotor) = 0; + virtual void receivePosition (double command) = 0; + virtual void receivePosX (double xPos) = 0; + virtual void receivePosY (double yPos) = 0; + virtual void receivePosA (double aPos) = 0; + virtual void receiveMotor (int oldLSpeed_e, int LSpeed_int, int oldRSpeed_e, int RSpeed_int) = 0; + virtual void receivePwm (double leftPwm, double rightPwm) = 0; + virtual void receiveTiming (double motorTimer5, double motorTimer4, + double motorTimer3, double motorTimer2, + double motorTimer1, + double motorTimer0 ) = 0; + virtual void receiveInPort (double command) = 0; + virtual void jaifini (void) = 0; }; + // Communication avec l'AVR Proto proto_; std::string ttyName_; + Receiver & receiver_; // Paramètre de l'avr int16_t footing_; double epsilon_; @@ -89,6 +97,7 @@ class Asserv : public NonCopyable, public Proto::Receiver void fuckTheWall(double speed); void setSpeed(double xSpeed = 0, double ySpeed = 0); void setPwm(double leftPwm = 0, double rightPwm = 0); + void finishAck(void); //met le paramètre F à 0 /// Statistiques void statCounter(double period = 0); void statPosition(double period = 0); @@ -98,7 +107,6 @@ class Asserv : public NonCopyable, public Proto::Receiver void statInPort(double period = 0); //@{ /// Change les paramètres de la carte. - void fermeTaGueule(void); //met le paramètre F à 0 void setXPos(double pos); void setYPos(double pos); void setAngle(double angle); @@ -107,6 +115,7 @@ class Asserv : public NonCopyable, public Proto::Receiver void setKp (double kp); void setKi (double ki); void setKd (double kd); + void setSpeedIntMax (int16_t maxInt); void setAccel (double accel); void setMaxSpeed(double maxSpeed); //@} @@ -116,7 +125,7 @@ class Asserv : public NonCopyable, public Proto::Receiver private: /// Charge les données de la classe config - void loadConfig(void); + void loadConfig(const Config & config); /// Fonctions de conversion int mm2pasD(double dist); //Format 24/8 int radTo256(double angle, bool format24eme = false); //Format 8 ou 8/24 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