From dcb901c549f81eb8124458448ce40e305a98fd4d Mon Sep 17 00:00:00 2001 From: haller Date: Sun, 17 Apr 2005 02:32:07 +0000 Subject: Corrections de quelques erreurs de syntaxe classe à tester avec un port série --- 2005/i/robert/src/asserv/Makefile.defs | 2 +- 2005/i/robert/src/asserv/asserv.cc | 19 +- 2005/i/robert/src/asserv/asserv.hh | 197 ++++++++------- 2005/i/robert/src/asserv/test_asserv.cc | 433 ++++++++++++++++---------------- 4 files changed, 332 insertions(+), 319 deletions(-) (limited to '2005') diff --git a/2005/i/robert/src/asserv/Makefile.defs b/2005/i/robert/src/asserv/Makefile.defs index 23a9993..80bf8ee 100644 --- a/2005/i/robert/src/asserv/Makefile.defs +++ b/2005/i/robert/src/asserv/Makefile.defs @@ -2,6 +2,6 @@ PROGRAMS += test_asserv asserv_OBJECTS = asserv.o -test_asserv_OBJECTS = test_asserv.o $(asserv_OBJECTS) $(log_OBJECTS) +test_asserv_OBJECTS = test_asserv.o $(asserv_OBJECTS) $(config_OBJECTS) $(timer_OBJECTS) $(proto_OBJECTS) $(log_OBJECTS) $(serial_OBJECTS) $(utils_OBJECTS) test_asserv: $(test_asserv_OBJECTS) diff --git a/2005/i/robert/src/asserv/asserv.cc b/2005/i/robert/src/asserv/asserv.cc index e3a0f59..358a3fc 100644 --- a/2005/i/robert/src/asserv/asserv.cc +++ b/2005/i/robert/src/asserv/asserv.cc @@ -241,6 +241,7 @@ void Asserv::setKd(double Kd) void Asserv::setSpeedIntMax(int16_t maxInt) { + speedIntMax_ = maxInt; proto_.send('p', "bW", 'a', maxInt); } @@ -334,14 +335,16 @@ void Asserv::receive(char command, const Proto::Frame &frame) /// Charge les données de la classe config 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"); + ttyName_ = config.get("asserv.tty"); + 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"); + speedIntMax_ = config.get("asserv.speedIntMax"); + maxSpeed_ = config.get("asserv.maxSpeed"); + pwmMax_ = config.get("asserv.pwmMax"); } int Asserv::mm2pasD(double dist) diff --git a/2005/i/robert/src/asserv/asserv.hh b/2005/i/robert/src/asserv/asserv.hh index ef6a715..34e6f06 100644 --- a/2005/i/robert/src/asserv/asserv.hh +++ b/2005/i/robert/src/asserv/asserv.hh @@ -33,106 +33,109 @@ /// Classe de dialogue avec la carte d'asservissement. class Asserv : public NonCopyable, public Proto::Receiver { - /// Les clients de Asserv doivent dériver de Receiver. - class Receiver - { - public: - /// Recoit un packet. - 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_; - double accel_; - double kp_; - double ki_; - double kd_; - double maxSpeed_; - double maxPwn_; - // Stat - double pCounter_; - double pPosition_; - double pMotor_; - double pPwm_; - double pTiming_; - double pStatInPort_; + public: + /// Les clients de Asserv doivent dériver de Receiver. + class Receiver + { + public: + /// Recoit un packet. + virtual void receiveCounter (double lMotor, double rMotor) = 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; + }; - // Unités - double mmPpas_; - double sPperiod_; - double pwmMax_; - - // Système de log. - Log log_; - public: - /// Constructeur. - Asserv (const Config & config, Asserv::Receiver & receiver); - /// Reset la carte et envois les paramètres. - void reset (void); - /// Essaie de purger la liste d'émission et indique si elle est vide. - bool sync (void); - /// Attend que toute les émissions soit terminées. - bool wait (int timeout = -1); - /// Commandes asservissement - void linearMove(double distance); - void angularMove(double angle); - void goToPosition(double xPos, double yPos); - 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); - void statMotor(double period = 0); - void statPwm(double period = 0); - void statTiming(double period = 0); - void statInPort(double period = 0); - //@{ - /// Change les paramètres de la carte. - void setXPos(double pos); - void setYPos(double pos); - void setAngle(double angle); - void setFooting(int16_t dist); - void setEpsilon(double e); - 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); - //@} + private: - /// implémentation du proto::Receiver - void receive (char command, const Proto::Frame &frame); + // Communication avec l'AVR + Proto proto_; + std::string ttyName_; + Receiver & receiver_; + // Paramètre de l'avr + int16_t footing_; + double epsilon_; + double accel_; + double kp_; + double ki_; + double kd_; + int16_t speedIntMax_; + double maxSpeed_; + double maxPwn_; + // Stat + double pCounter_; + double pPosition_; + double pMotor_; + double pPwm_; + double pTiming_; + double pStatInPort_; - private: - /// Charge les données de la classe config - 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 - int mms2ppp(double vitesse); // Format 8 - int rCycl2Pwm(double rCycl); // Format 16 - int s2period(double period); // Format 8 - int mmps2ppperiod(double accel); //Format 8 (1 vitesse par x period) + // Unités + double mmPpas_; + double sPperiod_; + double pwmMax_; + + // Système de log. + Log log_; + public: + /// Constructeur. + Asserv (const Config & config, Asserv::Receiver & receiver); + /// Reset la carte et envois les paramètres. + void reset (void); + /// Essaie de purger la liste d'émission et indique si elle est vide. + bool sync (void); + /// Attend que toute les émissions soit terminées. + bool wait (int timeout = -1); + /// Commandes asservissement + void linearMove(double distance); + void angularMove(double angle); + void goToPosition(double xPos, double yPos); + 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); + void statMotor(double period = 0); + void statPwm(double period = 0); + void statTiming(double period = 0); + void statInPort(double period = 0); + //@{ + /// Change les paramètres de la carte. + void setXPos(double pos); + void setYPos(double pos); + void setAngle(double angle); + void setFooting(int16_t dist); + void setEpsilon(double e); + 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); + //@} + + /// implémentation du proto::Receiver + void receive (char command, const Proto::Frame &frame); + + private: + /// Charge les données de la classe config + 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 + int mms2ppp(double vitesse); // Format 8 + int rCycl2Pwm(double rCycl); // Format 16 + int s2period(double period); // Format 8 + int mmps2ppperiod(double accel); //Format 8 (1 vitesse par x period) }; #endif // asserv_h diff --git a/2005/i/robert/src/asserv/test_asserv.cc b/2005/i/robert/src/asserv/test_asserv.cc index d7c8964..54e5716 100644 --- a/2005/i/robert/src/asserv/test_asserv.cc +++ b/2005/i/robert/src/asserv/test_asserv.cc @@ -25,252 +25,259 @@ // // }}} +#include "asserv.hh" +#include "config/config.hh" +#include "timer/timer.hh" + +#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:" << "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; - } + 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 + void syntax (void) { std::cout << "test_asserv - test la classe de protocol asserv.\n" - "Syntaxe : test_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(config, testAsserv); - //proto.open (argv[1]); - // XXX Charger la conf - i = 2; + i = 1; 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; + { + 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], 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(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(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(strtol(argv[i], 0, 10)); + 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); + 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 (); - } - break; - } - case '?': - syntax (); - return 0; - default: - throw std::runtime_error ("syntax error"); - } + 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; } -- cgit v1.2.3