// asserv.cc // 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. // // }}} #include "asserv.hh" #include /// Constructeur Asserv::Asserv (const Config & config, Receiver & receiver) :proto_(*this), receiver_(receiver), log_("asserv") { // Rechargement des paramètres loadConfig(config); //Ouverture du port série proto_.open(ttyName_); } /// Reset de l'AVR void Asserv::reset(void) { // Reset AVR proto_.send('z'); // Envoie des données de conf sur l'AVR setFooting(footing_); setEpsilon(epsilon_); setKp(kp_); setKi(ki_); setKd(kd_); setESat(eSat_); setSpeedIntMax(speedIntMax_); setDSample(dSample_); setAccel(accel_); setMaxSpeed(maxSLin_, maxSRot_); 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. bool Asserv::sync(void) { return proto_.sync(); } /// Attend que toute les émissions soit terminées bool Asserv::wait(int timeout) { return proto_.wait(timeout); } /// Récupère le File Descriptor int Asserv::getFd(void) { return proto_.getFd(); } /// Commandes d'asservissement void Asserv::linearMove(double distance) { // Conversion mm->PasW int distPas = mm2pas(distance); log_ ("linearMove") << "avConv" << distance << "apConv" << distPas; // On envoie la commande à l'AVR proto_.send('l', "w", distPas); } 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); } void Asserv::goToPosition(double xPos, double yPos) { // Convertion mm->PasD 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); } // XXX Vérifier la conversion ca me parais un peu petit void Asserv::fuckTheWall(double speed) { // Conversion mm/s->Pas/Period int v = mms2ppp(speed); log_ ("fuckTheWall") << "avConv" << speed << "apConv" << v; // Envoie vers l'AVR 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) { // 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 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(int xPwm, int yPwm) { log_ ("setPwm") << "avConvX" << xPwm; log_ ("setPwm") << "avConvY" << yPwm; // Envoie sur l'AVR proto_.send('w',"ww" ,xPwm, yPwm); } /// Informe l'AVR d'arréter de dire "j'ai fini" void Asserv::finishAck(void) { proto_.send('F', "b", 2); } /// XXX Valeurs bizarres /// Statistiques 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(int period) { pPosition_ = period; // Convertion s->Period int p = s2period(period); // Envoie sur l'AVR proto_.send('X', "b", p); } void Asserv::statMotor(int period) { pMotor_ = period; // Convertion s->Period int p = s2period(period); // Envoie sur l'AVR proto_.send('S', "b", p); } void Asserv::statPwm(int period) { pPwm_ = period; // Convertion s->Period int p = s2period(period); // Envoie sur l'AVR proto_.send('W', "b", p); } void Asserv::statTiming(int period) { pTiming_ = period; // Convertion s->Period int p = s2period(period); // Envoie sur l'AVR proto_.send('T', "b", p); } void Asserv::statInPort(int period) { pStatInPort_ = period; // Convertion s->Period int p = s2period(period); // Envoie sur l'AVR 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 = mm2pas(xPos, true); // Envoie à l'AVR proto_.send('p',"bd", 'x', p); } void Asserv::setYPos(double yPos) { // Conversion mm->pas int p = mm2pas(yPos, true); // Envoie à l'AVR proto_.send('p',"bd", 'y', p); } void Asserv::setAngle(double angle) { // Envoie à l'AVR proto_.send('p',"bd", 'a', static_cast(angle / (2 * M_PI) * (1 << 24))); } void Asserv::setFooting(int16_t footing) { footing_ = footing; // Envoie à l'AVR proto_.send('p',"bw", 'f', footing); } void Asserv::setEpsilon(double epsilon) { epsilon_ = epsilon; // Envoie à l'AVR proto_.send('p',"bd", 'e', static_cast(epsilon * 256)); } void Asserv::setKp(int Kp) { kp_ = Kp; // Envoie à l'AVR proto_.send('p',"bw", 'p', Kp); } void Asserv::setKi(int Ki) { ki_ = Ki; // Envoie à l'AVR proto_.send('p',"bw", 'i', Ki); } void Asserv::setKd(int Kd) { kd_ = Kd; // Envoie à l'AVR proto_.send('p',"bw", 'd', Kd); } void Asserv::setESat(int eSat) { eSat_ = eSat; // Envoie à l'AVR proto_.send('p', "bw", 'E', eSat); } void Asserv::setSpeedIntMax(int maxInt) { speedIntMax_ = maxInt; proto_.send('p', "bW", 'I', maxInt); } void Asserv::setDSample (int dSample) { dSample_ = dSample; proto_.send('p', "bb", 's', dSample); } void Asserv::setAccel(int accel) { accel_ = accel_; log_ ("setAccel") << "avConv" << accel; // envoie vers l'AVR 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_; // XXX Vérifier ca } /// Règle les vitesses maximum(lineaire et rotation) void Asserv::setMaxSpeed(int maxSLin, int maxSRot) { maxSLin_ = maxSLin; maxSRot_ = maxSRot; // Envoie à l'AVR proto_.send('p',"bbb", 'm', maxSLin, maxSRot); } 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 , rightInvPwm); } /// implémentation du proto::Receiver void Asserv::receive(char command, const Proto::Frame &frame) { 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 receiver_.receivePosX(xPos * mmPpas_ / 256); break; case 'Y': int yPos; proto_.decode(frame, "D", yPos); // Convertion Pas->mm receiver_.receivePosY(yPos * mmPpas_ / 256); break; case 'A': { int aPos; proto_.decode(frame, "D", aPos); double aPosD = (aPos * (2 * M_PI) / (1 << 24)); receiver_.receivePosA(aPosD); break; } case 'S': int oldLSpeed, LSpeed, oldRSpeed, RSpeed; proto_.decode(frame, "WWWW", oldLSpeed, LSpeed, oldRSpeed, RSpeed); receiver_.receiveSpeedStat(oldLSpeed, LSpeed, oldRSpeed, RSpeed); break; case 'W': int leftPwm, rightPwm; proto_.decode(frame, "WW", leftPwm, rightPwm); receiver_.receivePwm(leftPwm, rightPwm); break; case 'T': { 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(motorTimer4, motorTimer3, motorTimer2, motorTimer1, motorTimer0); break; } case 'P': { int statePIn; proto_.decode(frame, "b", statePIn); 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_.done(); break; } } /// Charge les données de la classe config 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"); kp_ = config.get("asserv.kp"); ki_ = config.get("asserv.ki"); kd_ = config.get("asserv.kd"); eSat_ = config.get("asserv.eSat"); speedIntMax_ = config.get("asserv.speedIntMax"); dSample_ = config.get("asserv.dSample"); maxSLin_ = config.get("asserv.maxSLin"); maxSRot_ = config.get("asserv.maxSRot"); useTazFSM_ = config.get("asserv.useTazFSM"); mmPpas_ = config.get("asserv.mmPpas"); sPperiod_ = config.get("asserv.sPperiod"); 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::mm2pas(double dist, bool format24eme) { 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)) * 256 * (1 << 24)); else return static_cast((angle / (2 * M_PI))* 256); } int Asserv::mms2ppp(double vitesse) { return static_cast(vitesse * sPperiod_ / mmPpas_); } int Asserv::rCycl2Pwm(double rCycl) { return static_cast(rCycl*pwmMax_); } int Asserv::s2period(double period) { return static_cast(period/sPperiod_); } //int Asserv::mmps2ppperiod(double accel) //{ // return static_cast(1/(accel*(sPperiod_*sPperiod_/mmPpas_))); //}