From a0d2e9173bcfcc37b51e9ad51e5e8cd3c2206656 Mon Sep 17 00:00:00 2001 From: haller Date: Tue, 19 Apr 2005 00:37:37 +0000 Subject: Correction de bug Ajout des commandes H(statSharp), p'W'(param InvPwm) Ajout du traitement de la frame !t par l'AVR(TazState) Modification de format de certaine variable(double->int) --- 2005/i/robert/src/asserv/asserv.cc | 157 +++++++++++++++++++++++--------- 2005/i/robert/src/asserv/asserv.hh | 78 ++++++++-------- 2005/i/robert/src/asserv/test_asserv.cc | 80 ++++++++++------ 3 files changed, 212 insertions(+), 103 deletions(-) (limited to '2005/i') diff --git a/2005/i/robert/src/asserv/asserv.cc b/2005/i/robert/src/asserv/asserv.cc index c1be051..fc17d9e 100644 --- a/2005/i/robert/src/asserv/asserv.cc +++ b/2005/i/robert/src/asserv/asserv.cc @@ -47,17 +47,21 @@ void Asserv::reset(void) // Envoie des données de conf sur l'AVR setFooting(footing_); setEpsilon(epsilon_); - setAccel(accel_); setKp(kp_); setKi(ki_); setKd(kd_); + setSpeedIntMax(speedIntMax_); + setAccel(accel_); setMaxSpeed(maxSpeed_); + setUseTazFSM(useTazFSM_); + setInvPwm(leftInvPwm_, rightInvPwm_); statCounter(pCounter_); statPosition(pPosition_); statMotor(pMotor_); statPwm(pPwm_); statTiming(pTiming_); statInPort(pStatInPort_); + statSharp(pStatInPort_); } /// Essaie de purger la liste d'émission et indique si elle est vide. @@ -76,7 +80,8 @@ bool Asserv::wait(int timeout) void Asserv::linearMove(double distance) { // Conversion mm->PasW - int distPas = mm2pasD(distance); + int distPas = mm2pas(distance); + log_ ("linearMove") << "avConv" << distance << "apConv" << distPas; // On envoie la commande à l'AVR proto_.send('l', "w", distPas); } @@ -85,6 +90,7 @@ void Asserv::angularMove(double angle) { // Conversion radian->256ème int a = radTo256(angle); + log_ ("angularMove") << "avConv" << angle << "apConv" << a; // Envopie vers avr proto_.send('a',"b", a); } @@ -92,8 +98,10 @@ void Asserv::angularMove(double angle) void Asserv::goToPosition(double xPos, double yPos) { // Convertion mm->PasD - int x = mm2pasD(xPos); - int y = mm2pasD(yPos); + int x = mm2pas(xPos, true); + int y = mm2pas(yPos, true); + log_ ("goToPosition") << "avConvX" << xPos << "apConvX" << x; + log_ ("goToPosition") << "avConvY" << yPos << "apConvY" << y; // Envoie vers l'AVR proto_.send('g', "dd", x, y); } @@ -102,8 +110,16 @@ void Asserv::fuckTheWall(double speed) { // Conversion mm/s->Pas/Period int v = mms2ppp(speed); + log_ ("fuckTheWall") << "avConv" << speed << "apConv" << v; // Envoie vers l'AVR - proto_.send('f',"b", v); + fuckTheWall(v); +} + +void Asserv::fuckTheWall(int speed) +{ + log_ ("fuckTheWall") << "avConv"; + // Envoie vers l'AVR + proto_.send('f',"b", speed); } void Asserv::setSpeed(double xSpeed, double ySpeed) @@ -111,8 +127,18 @@ void Asserv::setSpeed(double xSpeed, double ySpeed) // Conversion mm/s->Pas/Period int vx = mms2ppp(xSpeed); int vy = mms2ppp(ySpeed); + log_ ("setSpeed") << "avConvX" << xSpeed << "apConvX" << vx; + log_ ("setSpeed") << "avConvY" << ySpeed << "apConvY" << vy; // Envoie vers l'AVR - proto_.send('s',"bb", vx, vy); + setSpeed(vx,vy); +} + +void Asserv::setSpeed(int xSpeed, int ySpeed) +{ + log_ ("setSpeed") << "avConvX" << xSpeed ; + log_ ("setSpeed") << "avConvY" << ySpeed ; + // Envoie vers l'AVR + proto_.send('s',"bb", xSpeed, ySpeed); } void Asserv::setPwm(double xPwm, double yPwm) @@ -120,6 +146,8 @@ void Asserv::setPwm(double xPwm, double yPwm) // Conversion rCycl->Pwm int xp = rCycl2Pwm(xPwm); int yp = rCycl2Pwm(yPwm); + log_ ("setPwm") << "avConvX" << xPwm << "apConvX" << xp; + log_ ("setPwm") << "avConvY" << yPwm << "apConvY" << yp; // Envoie sur l'AVR proto_.send('w',"ww" ,xp, yp); } @@ -131,16 +159,17 @@ void Asserv::finishAck(void) } /// Statistiques -void Asserv::statCounter(double period) +void Asserv::statCounter(int period) { pCounter_ = period; // Convertion s->Period int p = s2period(period); + log_ ("statCounter") << "avConv" << period << "apConv" << p; // Envoie sur l'AVR proto_.send('C', "b", p); } -void Asserv::statPosition(double period) +void Asserv::statPosition(int period) { pPosition_ = period; // Convertion s->Period @@ -149,7 +178,7 @@ void Asserv::statPosition(double period) proto_.send('X', "b", p); } -void Asserv::statMotor(double period) +void Asserv::statMotor(int period) { pMotor_ = period; // Convertion s->Period @@ -158,7 +187,7 @@ void Asserv::statMotor(double period) proto_.send('S', "b", p); } -void Asserv::statPwm(double period) +void Asserv::statPwm(int period) { pPwm_ = period; // Convertion s->Period @@ -167,7 +196,7 @@ void Asserv::statPwm(double period) proto_.send('W', "b", p); } -void Asserv::statTiming(double period) +void Asserv::statTiming(int period) { pTiming_ = period; // Convertion s->Period @@ -176,7 +205,7 @@ void Asserv::statTiming(double period) proto_.send('T', "b", p); } -void Asserv::statInPort(double period) +void Asserv::statInPort(int period) { pStatInPort_ = period; // Convertion s->Period @@ -185,11 +214,20 @@ void Asserv::statInPort(double period) proto_.send('P', "b", p); } +void Asserv::statSharp(int period) +{ + pSharp_ = period; + // Conversion s->period + int p = s2period(period); + // Envoie sur l'AVR + proto_.send('H', "b", p); +} + /// Change les paramètres void Asserv::setXPos(double xPos) { // Conversion mm->pas - int p = mm2pasD(xPos); + int p = mm2pas(xPos); // Envoie à l'AVR proto_.send('p',"bd", 'x', p); } @@ -197,7 +235,7 @@ void Asserv::setXPos(double xPos) void Asserv::setYPos(double yPos) { // Conversion mm->pas - int p = mm2pasD(yPos); + int p = mm2pas(yPos); // Envoie à l'AVR proto_.send('p',"bd", 'y', p); } @@ -205,7 +243,7 @@ void Asserv::setYPos(double yPos) void Asserv::setAngle(double angle) { // Conversion mm->pas - int a = mm2pasD(angle); + int a = radTo256(angle); // Envoie à l'AVR proto_.send('p',"bd", 'y', a); } @@ -245,28 +283,50 @@ void Asserv::setKd(double Kd) proto_.send('p',"bw", 'd', static_cast(Kd * 256)); } -void Asserv::setSpeedIntMax(int16_t maxInt) +void Asserv::setSpeedIntMax(int maxInt) { speedIntMax_ = maxInt; proto_.send('p', "bW", 'a', maxInt); } -void Asserv::setAccel(double accel) +void Asserv::setAccel(int accel) { - // Conversion mmps2ppperiod - int a = mmps2ppperiod(accel); + accel_ = accel_; + log_ ("setAccel") << "avConv" << accel; // envoie vers l'AVR - proto_.send('p',"bb", 'a', a); + proto_.send('p',"bb", 'a', accel); +} + +/// Renvoie l'accélération en unité international (mm/s²) +double Asserv::getAccel(void) +{ + return mmPpas_ / (sPperiod_ * sPperiod_) / accel_; } -void Asserv::setMaxSpeed(double maxSpeed) +void Asserv::setMaxSpeed(int maxSpeed) { + maxSpeed_ = maxSpeed; // Conversion mm->ppp int p = mms2ppp(maxSpeed); // Envoie à l'AVR proto_.send('p',"bb", 'm', p); } +void Asserv::setUseTazFSM(bool use) +{ + useTazFSM_ = use; + // Envoie la commande à l'AVR + proto_.send('t', "b", use ? 1 : 0); +} + +void Asserv::setInvPwm(bool leftInvPwm, bool rightInvPwm) +{ + leftInvPwm_ = leftInvPwm; + rightInvPwm_ = rightInvPwm; + // Envoie la commande à l'AVR + proto_.send('p', "bbb", 'W', leftInvPwm ? 1 : 0, rightInvPwm ? 1 : 0); +} + /// implémentation du proto::Receiver void Asserv::receive(char command, const Proto::Frame &frame) { @@ -302,7 +362,7 @@ void Asserv::receive(char command, const Proto::Frame &frame) case 'S': int oldLSpeed, LSpeed, oldRSpeed, RSpeed; proto_.decode(frame, "WWWW", oldLSpeed, LSpeed, oldRSpeed, RSpeed); - receiver_.receiveMotor(oldLSpeed, LSpeed, oldRSpeed, RSpeed); + receiver_.receiveSpeedStat(oldLSpeed, LSpeed, oldRSpeed, RSpeed); break; case 'W': int leftPwm, rightPwm; @@ -311,18 +371,13 @@ void Asserv::receive(char command, const Proto::Frame &frame) 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_); + receiver_.receiveTiming(motorTimer4, motorTimer3, motorTimer2, + motorTimer1, motorTimer0); break; } case 'P': @@ -332,8 +387,21 @@ void Asserv::receive(char command, const Proto::Frame &frame) receiver_.receiveInPort(statePIn); break; } + case 'H': + { + int stateSharp0, stateSharp1, stateSharp2; + proto_.decode(frame, "www", stateSharp0, stateSharp1, stateSharp2); + receiver_.receiveSharp(stateSharp0, stateSharp1, stateSharp2); + break; + } + case 't': + { + int state, subState; + proto_.decode(frame, "bb", state, subState); + receiver_.receiveTazState(state, subState); + } case 'F': - receiver_.jaifini(); + receiver_.done(); break; } } @@ -344,34 +412,41 @@ void Asserv::loadConfig(const Config & config) ttyName_ = config.get("asserv.tty"); footing_ = config.get("asserv.footing"); epsilon_ = config.get("asserv.epsilon"); - accel_ = config.get("asserv.accel"); + 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"); + maxSpeed_ = config.get("asserv.maxSpeed"); + useTazFSM_ = config.get("asserv.useTazFSM"); mmPpas_ = config.get("asserv.mmPpas"); sPperiod_ = config.get("asserv.sPperiod"); - pwmMax = config.get("asserv.pwmMax"); + pwmMax_ = config.get("asserv.pwmMax"); + leftInvPwm_ = config.get("asserv.leftInvPwm"); + rightInvPwm_ = config.get("asserv.rightInvPwm"); pCounter_ = 0; pPosition_ = 0; pMotor_ = 0; pPwm_ = 0; pTiming_ = 0; pStatInPort_ = 0; + pSharp_ = 0; } -int Asserv::mm2pasD(double dist) +int Asserv::mm2pas(double dist, bool format24eme) { - return static_cast((dist/mmPpas_)*256); + if(format24eme) + return static_cast((dist/mmPpas_)*256); + else + return static_cast(dist/mmPpas_); } int Asserv::radTo256(double angle, bool format24eme) { if(format24eme) - return static_cast((angle / (2 * M_PI)) * (1 << 24)); + return static_cast((angle / (2 * M_PI)) * 256 * (1 << 24)); else - return static_cast(angle / (2 * M_PI)); + return static_cast((angle / (2 * M_PI))* 256); } int Asserv::mms2ppp(double vitesse) @@ -389,7 +464,7 @@ int Asserv::s2period(double period) return static_cast(period/sPperiod_); } -int Asserv::mmps2ppperiod(double accel) -{ - return static_cast(1/(accel*(sPperiod_*sPperiod_/mmPpas_))); -} +//int Asserv::mmps2ppperiod(double accel) +//{ +// return static_cast(1/(accel*(sPperiod_*sPperiod_/mmPpas_))); +//} diff --git a/2005/i/robert/src/asserv/asserv.hh b/2005/i/robert/src/asserv/asserv.hh index 4a19493..1cbe2c4 100644 --- a/2005/i/robert/src/asserv/asserv.hh +++ b/2005/i/robert/src/asserv/asserv.hh @@ -43,14 +43,16 @@ class Asserv : public NonCopyable, public Proto::Receiver 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 receiveSpeedStat (int leftError, int leftInt, int rightError, int rightInt) = 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; + virtual void receiveTiming (int motorTimer4, + int motorTimer3, int motorTimer2, + int motorTimer1, + int motorTimer0 ) = 0; + virtual void receiveInPort (int port) = 0; + virtual void receiveSharp (int sharp1, int sharp2, int sharp3) = 0; + virtual void receiveTazState(int state, int subState){}; // XXX Vérifier les formats + virtual void done (void) = 0; }; private: @@ -60,26 +62,29 @@ class Asserv : public NonCopyable, public Proto::Receiver std::string ttyName_; Receiver & receiver_; // Paramètre de l'avr - int16_t footing_; - double epsilon_; - double accel_; - double kp_; + int footing_; //truc + double epsilon_; //mm + int accel_; //unité AVR + double kp_; double ki_; double kd_; - int16_t speedIntMax_; - double maxSpeed_; + int speedIntMax_; + int maxSpeed_; //unité AVR + bool leftInvPwm_, rightInvPwm_; + bool useTazFSM_; // Stat - double pCounter_; - double pPosition_; - double pMotor_; - double pPwm_; - double pTiming_; - double pStatInPort_; + int pCounter_; + int pPosition_; + int pMotor_; + int pPwm_; + int pTiming_; + int pStatInPort_; + int pSharp_; // Unités double mmPpas_; double sPperiod_; - double pwmMax_; + int pwmMax_; // Système de log. Log log_; @@ -97,16 +102,19 @@ class Asserv : public NonCopyable, public Proto::Receiver void angularMove(double angle); void goToPosition(double xPos, double yPos); void fuckTheWall(double speed); + void fuckTheWall(int speed); void setSpeed(double xSpeed = 0, double ySpeed = 0); + void setSpeed(int xSpeed = 0, int 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); + void statCounter(int period = 0); + void statPosition(int period = 0); + void statMotor(int period = 0); + void statPwm(int period = 0); + void statTiming(int period = 0); + void statInPort(int period = 0); + void statSharp(int period = 0); //@{ /// Change les paramètres de la carte. void setXPos(double pos); @@ -117,9 +125,12 @@ 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); + void setSpeedIntMax (int maxInt); + void setAccel (int accel); + double getAccel (void); + void setMaxSpeed(int maxSpeed); + void setUseTazFSM(bool use); + void setInvPwm(bool leftInvPwm, bool rightInvPwm); //@} /// implémentation du proto::Receiver @@ -129,17 +140,12 @@ class Asserv : public NonCopyable, public Proto::Receiver /// Charge les données de la classe config void loadConfig(const Config & config); /// Fonctions de conversion - int mm2pasD(double dist); //Format 24/8 + int mm2pas(double dist, bool format24eme = false); //Format 24/8 ou 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) + //int mmps2ppperiod(double accel); //Format 8 (1 vitesse par x period) }; #endif // asserv_h - - -// Vérifier le format finale des conversions -// Vérifier toutes les commandes passé à l'AVR -// Faire la fonction receive diff --git a/2005/i/robert/src/asserv/test_asserv.cc b/2005/i/robert/src/asserv/test_asserv.cc index 54e5716..302be6a 100644 --- a/2005/i/robert/src/asserv/test_asserv.cc +++ b/2005/i/robert/src/asserv/test_asserv.cc @@ -39,47 +39,60 @@ class TestAsserv : public Asserv::Receiver { 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) + + void receiveSpeedStat (int leftError, int leftInt, int rightError, int rightInt) { - std::cout << "S:" << "oldLSpeed_e:" << oldLSpeed_e << "LSpeed_int:" << - LSpeed_int << "oldRSpeed_e:" << oldRSpeed_e << - "RSpeed_int:" << RSpeed_int << std::endl; + 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 (double motorTimer5, double motorTimer4, - double motorTimer3, double motorTimer2, - double motorTimer1, - double motorTimer0 ) + + void receiveTiming (int motorTimer4, int motorTimer3, + int motorTimer2, int motorTimer1, + int motorTimer0 ) { - std::cout << "T:" << "motorTimer5:" << motorTimer5 << - " motorTimer4:" << motorTimer4 << "motorTimer3:" << - motorTimer3 << "motorTimer2:" << motorTimer2 << - "motorTimer1:" << motorTimer1 << "motorTimer0:" << - motorTimer0 << std::endl; + std::cout << "T:" << " motorTimer4:" << motorTimer4 << "motorTimer3:" + << motorTimer3 << "motorTimer2:" << motorTimer2 + << "motorTimer1:" << motorTimer1 << "motorTimer0:" + << motorTimer0 << std::endl; } - void receiveInPort (double state) + + void receiveInPort (int port) { - std::cout << "P:" << "state:" << state << std::endl; + std::cout << "P:" << "state:" << port << std::endl; } - void jaifini (void) + + 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; } + }; /// Affiche un memo de suntaxe. @@ -133,12 +146,14 @@ main (int argc, char **argv) case 'g': if(++i + 1 > argc) throw std::runtime_error("syntax error"); - asserv.goToPosition(strtod(argv[i], 0), strtod(argv[++i], 0)); + asserv.goToPosition(strtod(argv[i], 0), + strtod(argv[i + 1], 0)); + i++; break; case 'f': if(++i > argc) throw std::runtime_error("syntax error"); - asserv.fuckTheWall(strtod(argv[i], 0)); + asserv.fuckTheWall(atoi(argv[i])); break; case 's': if(++i + 1 > argc) @@ -156,33 +171,40 @@ main (int argc, char **argv) case 'C': if(++i > argc) throw std::runtime_error("syntax error"); - asserv.statCounter(strtod(argv[i], 0)); + asserv.statCounter(strtol(argv[i], 0, 10)); break; case 'X': if(++i > argc) throw std::runtime_error("syntax error"); - asserv.statPosition(strtod(argv[i], 0)); + asserv.statPosition(strtol(argv[i], 0, 10)); break; case 'S': if(++i > argc) throw std::runtime_error("syntax error"); - asserv.statMotor(strtod(argv[i], 0)); + asserv.statMotor(strtol(argv[i], 0, 10)); break; case 'W': if(++i > argc) throw std::runtime_error("syntax error"); - asserv.statPwm(strtod(argv[i], 0)); + asserv.statPwm(strtol(argv[i], 0, 10)); break; case 'T': if(++i > argc) throw std::runtime_error("syntax error"); - asserv.statTiming(strtod(argv[i], 0)); + asserv.statTiming(strtol(argv[i], 0, 10)); break; case 'P': if(++i > argc) throw std::runtime_error("syntax error"); - asserv.fuckTheWall(strtod(argv[i], 0)); + asserv.fuckTheWall(atoi(argv[i])); 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"); @@ -236,12 +258,18 @@ main (int argc, char **argv) case 'A': if(++i > argc) throw std::runtime_error("syntax error"); - asserv.setAccel(strtod(argv[i], 0)); + asserv.setAccel(strtol(argv[i], 0, 10)); break; case 'm': if(++i > argc) throw std::runtime_error("syntax error"); - asserv.setMaxSpeed(strtod(argv[i], 0)); + asserv.setMaxSpeed(strtol(argv[i], 0, 10)); + break; + case 'W': + if (++i + 1 > argc) + throw std::runtime_error("syntax error"); + asserv.setInvPwm(atoi(argv[i]), atoi(argv[i + 1])); + i++; break; } } -- cgit v1.2.3